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 24 Packages Projects Releases Wiki Activity

removing old quad files

Browse Source
This commit is contained in:
Gautier Hattenberger
2008-09-29 15:24:19 +00:00
parent b2dc007998
commit 4311ed2df4
8 changed files with 0 additions and 1154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/quad_control.c
-199
View File
@@ -1,199 +0,0 @@
/*
* $Id$
*
* 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 "booz_control.h"
#include "airframe.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "radio_control.h"
#define BOOZ_CONTROL_MIN_THROTTLE 0.05
float booz_control_p_sp;
float booz_control_q_sp;
float booz_control_r_sp;
float booz_control_power_sp;
float booz_control_rate_pq_pgain;
float booz_control_rate_pq_dgain;
float booz_control_rate_r_pgain;
float booz_control_rate_r_dgain;
float booz_control_rate_last_err_p;
float booz_control_rate_last_err_q;
float booz_control_rate_last_err_r;
pprz_t booz_control_commands[COMMANDS_NB];
float booz_control_attitude_phi_sp;
float booz_control_attitude_phi_dot_sp;
float booz_control_attitude_theta_sp;
float booz_control_attitude_theta_dot_sp;
float booz_control_attitude_psi_sp;
float booz_control_attitude_phi_theta_pgain;
float booz_control_attitude_phi_theta_dgain;
float booz_control_attitude_psi_pgain;
float booz_control_attitude_psi_dgain;
#define BOOZ_CONTROL_ATTITUDE_XI 1.0
#define BOOZ_CONTROL_ATTITUDE_W 10.0
float booz_control_attitude_w;
#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.)
void booz_control_init(void) {
booz_control_p_sp = 0.;
booz_control_q_sp = 0.;
booz_control_r_sp = 0.;
booz_control_power_sp = 0.;
booz_control_rate_last_err_p = 0.;
booz_control_rate_last_err_q = 0.;
booz_control_rate_last_err_r = 0.;
booz_control_rate_pq_pgain = BOOZ_CONTROL_RATE_PQ_PGAIN;
booz_control_rate_pq_dgain = BOOZ_CONTROL_RATE_PQ_DGAIN;
booz_control_rate_r_pgain = BOOZ_CONTROL_RATE_R_PGAIN;
booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN;
booz_control_attitude_phi_sp = 0.;
booz_control_attitude_phi_dot_sp = 0.;
booz_control_attitude_theta_sp = 0.;
booz_control_attitude_theta_dot_sp = 0.;
booz_control_attitude_psi_sp = 0.;
booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN;
booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN;
booz_control_attitude_psi_pgain = BOOZ_CONTROL_ATTITUDE_PSI_PGAIN;
booz_control_attitude_psi_dgain = BOOZ_CONTROL_ATTITUDE_PSI_DGAIN;
booz_control_attitude_w = BOOZ_CONTROL_ATTITUDE_W;
}
void booz_control_rate_read_setpoints_from_rc(void) {
booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
void booz_control_rate_run(void) {
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp;
const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p;
booz_control_rate_last_err_p = rate_err_p;
const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p );
const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp;
const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q;
booz_control_rate_last_err_q = rate_err_q;
const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q );
const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
void booz_control_attitude_read_setpoints_from_rc(void) {
booz_control_attitude_phi_dot_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * (booz_control_attitude_w*booz_control_attitude_w *
(-rc_values[RADIO_ROLL]*RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ - booz_control_attitude_phi_sp)
- 2.*BOOZ_CONTROL_ATTITUDE_XI*booz_control_attitude_w * booz_control_attitude_phi_dot_sp);
booz_control_attitude_phi_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * booz_control_attitude_phi_dot_sp;
booz_control_attitude_theta_dot_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * (booz_control_attitude_w*booz_control_attitude_w *
(rc_values[RADIO_PITCH]*RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ - booz_control_attitude_theta_sp)
- 2*BOOZ_CONTROL_ATTITUDE_XI*booz_control_attitude_w * booz_control_attitude_theta_dot_sp);
booz_control_attitude_theta_sp += BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP * booz_control_attitude_theta_dot_sp;
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
}
else { /* BOOZ_AP_MODE_HEADING_HOLD */
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE)
booz_control_attitude_psi_sp = booz_estimator_psi;
else {
booz_control_attitude_psi_sp += -rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ;
}
}
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
void booz_control_attitude_run(void) {
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi +
booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta +
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
float cmd_r;
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
const float rate_err_r = booz_estimator_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
}
else {
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
booz_control_attitude_psi_dgain * booz_estimator_r;
}
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
sw/airborne/booz/quad_controller_main.c
-191
View File
@@ -1,191 +0,0 @@
/*
* $Id$
*
* 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 "booz_controller_main.h"
#include "std.h"
#include "init_hw.h"
#include "interrupt_hw.h"
#include "sys_time.h"
#include "led.h"
#include "booz_energy.h"
#include "commands.h"
#include "actuators.h"
#include "radio_control.h"
#include "adc.h"
#include "quad_ins.h"
#include "booz_estimator.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "booz_autopilot.h"
#include "uart.h"
#include "messages.h"
#include "downlink.h"
#include "quad_controller_telemetry.h"
#include "datalink.h"
int16_t trim_p = 0;
int16_t trim_q = 0;
int16_t trim_r = 0;
uint8_t vbat = 0;
uint32_t t0,diff;
#ifndef SITL
int main( void ) {
booz_controller_main_init();
while(1) {
if (sys_time_periodic())
booz_controller_main_periodic_task();
booz_controller_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_controller_main_init( void ) {
hw_init();
led_init();
sys_time_init();
adc_init();
booz_energy_init();
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
actuators_init();
SetCommands(commands_failsafe);
ppm_init();
radio_control_init();
quad_ins_init();
booz_estimator_init();
booz_control_init();
booz_nav_init();
booz_autopilot_init();
int_enable();
DOWNLINK_SEND_BOOT(&cpu_time_sec);
t0 = T0TC;
}
#define PeriodicPrescaleBy2( _code_0, _code_1) { \
static uint8_t _50hz = 0; \
_50hz++; \
if (_50hz >= 2) _50hz = 0; \
switch (_50hz) { \
case 0: \
_code_0; \
break; \
case 1: \
_code_1; \
break; \
} \
}
STATIC_INLINE void booz_controller_main_periodic_task( void ) {
t0 = T0TC;
quad_ins_periodic_task();
/* run control loops */
booz_autopilot_periodic_task();
// commands[COMMAND_P] = 0;
// commands[COMMAND_Q] = 0;
// commands[COMMAND_R] = 0;
// commands[COMMAND_THROTTLE] = MAX_PPRZ/3;
SetActuatorsFromCommands(commands);
PeriodicPrescaleBy2( \
{ \
radio_control_periodic_task(); \
if (rc_status != RC_OK) \
booz_autopilot_mode = BOOZ_AP_MODE_FAILSAFE; \
}, \
{ \
booz_energy_periodic(); \
booz_controller_telemetry_periodic_task(); \
} \
); \
diff = (T0TC - t0)/SYS_TICS_OF_USEC(1);
DOWNLINK_SEND_TIME(&diff);
}
STATIC_INLINE void booz_controller_main_event_task( void ) {
// FIXME
#ifndef SITL
#if DATALINK == PPRZ
if (PprzBuffer()) {
ReadPprzBuffer();
if (pprz_msg_received) {
pprz_parse_payload();
pprz_msg_received = FALSE;
}
}
#elif DATALINK == XBEE
if (XBeeBuffer()) {
ReadXBeeBuffer();
if (xbee_msg_received) {
xbee_parse_payload();
xbee_msg_received = FALSE;
}
}
#elif
#error "Unknown DATALINK"
#endif
if (dl_msg_available) {
dl_parse_msg();
dl_msg_available = FALSE;
}
//DlEventCheckAndHandle();
#endif
QuadInsEventCheckAndHandle(booz_estimator_read_inter_mcu_state);
RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event);
}
sw/airborne/booz/quad_controller_telemetry.c
-5
View File
@@ -1,5 +0,0 @@
#include "quad_controller_telemetry.h"
uint8_t telemetry_mode_Controller;
sw/airborne/booz/quad_controller_telemetry.h
-141
View File
@@ -1,141 +0,0 @@
#ifndef BOOZ_CONTROLLER_TELEMETRY_H
#define BOOZ_CONTROLLER_TELEMETRY_H
#include "std.h"
#include "messages.h"
#include "periodic.h"
#include "uart.h"
#include "booz_energy.h"
#include "radio_control.h"
#include "actuators.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "quad_gps.h"
#include "settings.h"
#include "downlink.h"
#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM)
#define PERIODIC_SEND_ATTITUDE() { \
int16_t phi = DegOfRad(booz_estimator_phi); \
int16_t psi = DegOfRad(booz_estimator_psi); \
int16_t theta = DegOfRad(booz_estimator_theta); \
DOWNLINK_SEND_ATTITUDE(&phi, &psi, &theta); \
}
#define PERIODIC_SEND_ESTIMATOR() DOWNLINK_SEND_ESTIMATOR(&booz_estimator_z, &booz_estimator_vz)
#define PERIODIC_SEND_BAT() { \
int16_t t = 0; \
uint16_t time = 0; \
uint8_t kill = 0; if (booz_autopilot_mode == BOOZ_AP_MODE_KILL) kill = 1; \
DOWNLINK_SEND_BAT(&t, &booz_energy_bat, &booz_estimator_flight_time, &kill, &time, &time, &booz_energy_current); \
}
#define PERIODIC_SEND_PPRZ_MODE() { \
uint8_t pprz_mode; \
switch (booz_autopilot_mode) { \
case BOOZ_AP_MODE_RATE: \
pprz_mode = 0; /*MANUAL*/ break; \
case BOOZ_AP_MODE_ATTITUDE: \
case BOOZ_AP_MODE_HEADING_HOLD: \
pprz_mode = 1; /*AUTO1*/ break; \
case BOOZ_AP_MODE_NAV: \
pprz_mode = 2; /*AUTO2*/ break; \
case BOOZ_AP_MODE_FAILSAFE: \
case BOOZ_AP_MODE_KILL: \
pprz_mode = 5; /*FAIL*/ break; \
} \
uint8_t gaz = 0; \
uint8_t lat = 0; \
uint8_t hor = 0; \
uint8_t calib = 0; \
uint8_t rc; \
switch (rc_status) { \
case RC_OK: \
rc = 1; /*OK*/ break; \
case RC_LOST: \
rc = 2; /*LOST*/ break; \
case RC_REALLY_LOST: \
rc = 0; /*NONE*/ break; \
} \
DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &gaz, &lat, &hor, &calib, &rc); \
}
#define PERIODIC_SEND_BOOZ_STATUS() \
DOWNLINK_SEND_QUAD_STATUS( \
&rc_status, \
&booz_autopilot_mode, \
&booz_energy_bat, \
&cpu_time_sec)
#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
#define PERIODIC_SEND_BOOZ_FD() \
DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \
&booz_estimator_q, \
&booz_estimator_r, \
&booz_estimator_phi, \
&booz_estimator_theta, \
&booz_estimator_psi);
#define PERIODIC_SEND_ACTUATORS() \
DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_CONTROL() { \
switch (booz_autopilot_mode) { \
case BOOZ_AP_MODE_RATE: \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
&booz_estimator_uf_q, &booz_control_q_sp, \
&booz_estimator_uf_r, &booz_control_r_sp, \
&booz_control_power_sp); \
break; \
case BOOZ_AP_MODE_ATTITUDE: \
case BOOZ_AP_MODE_HEADING_HOLD: \
case BOOZ_AP_MODE_NAV: \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
&booz_estimator_theta, &booz_control_attitude_theta_sp, \
&booz_estimator_psi, &booz_control_attitude_psi_sp, \
&booz_control_power_sp); \
break; \
} \
}
#define PERIODIC_SEND_BOOZ_VERT_LOOP() { \
float sp = -booz_nav_hover_z_sp; \
float z = -booz_estimator_z; \
float vz = -booz_estimator_w; \
DOWNLINK_SEND_BOOZ_VERT_LOOP(&sp,&vz,&z, \
&booz_nav_hover_power_command); \
}
#define PERIODIC_SEND_BOOZ_HOV_LOOP() { \
DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_hover_x_sp, \
&booz_nav_hover_y_sp, \
&booz_estimator_u, \
&booz_estimator_x, \
&booz_estimator_v, \
&booz_estimator_y, \
&booz_nav_hover_phi_command, \
&booz_nav_hover_theta_command); \
}
#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&actuators[SERVO_MOTOR_FRONT],&actuators[SERVO_MOTOR_BACK],&actuators[SERVO_MOTOR_RIGHT],&actuators[SERVO_MOTOR_LEFT])
#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
#define PERIODIC_SEND_GPS() gps_downlink();
extern uint8_t telemetry_mode_Controller;
static inline void booz_controller_telemetry_periodic_task(void) {
PeriodicSendController()
}
#endif /* BOOZ_CONTROLLER_TELEMETRY_H */
sw/airborne/booz/quad_gps.c
-97
View File
@@ -1,97 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2005 Pascal Brisset, 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.
*
*/
/** \file gps.c
* \brief GPS hardware independent handling
*
*/
#include <stdlib.h>
#include "quad_gps.h"
//#include "latlong.h"
#include "sys_time.h"
#include "airframe.h"
#include "uart.h"
#include "messages.h"
#include "downlink.h"
uint8_t gps_mode;
uint32_t gps_itow;
int32_t gps_alt;
uint16_t gps_gspeed;
int16_t gps_climb;
int16_t gps_course;
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
int32_t gps_lat, gps_lon;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
uint8_t gps_numSV;
uint8_t gps_configuring;
uint16_t gps_reset;
uint16_t last_gps_msg_t;
bool_t gps_verbose_downlink;
volatile bool_t gps_msg_received;
bool_t gps_pos_available;
uint8_t gps_nb_ovrn;
uint8_t gps_nb_channels;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
void gps_downlink( void ) {
static uint8_t _4Hz;
_4Hz++; if (_4Hz > 3) _4Hz = 0;
#if defined DOWNLINK_GPS_1Hz
if (_4Hz == 0)
#endif
DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone, &gps_nb_ovrn);
static uint8_t i;
static uint8_t last_cnos[GPS_NB_CHANNELS];
if (i >= gps_nb_channels) i = 0;
if (i < gps_nb_channels && gps_svinfos[i].cno > 0 && gps_svinfos[i].cno != last_cnos[i]) {
DOWNLINK_SEND_SVINFO(&i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim);
last_cnos[i] = gps_svinfos[i].cno;
}
if (gps_verbose_downlink) {
uint8_t j;
for(j = 0; j < gps_nb_channels; j++) {
uint8_t cno = gps_svinfos[j].cno;
if (cno > 0 && j != i && abs(cno-last_cnos[j]) >= 2) {
DOWNLINK_SEND_SVINFO(&j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim);
last_cnos[j] = gps_svinfos[j].cno;
}
}
}
i++;
}
sw/airborne/booz/quad_gps.h
-76
View File
@@ -1,76 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2003 Pascal Brisset, 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.
*
*/
/** \file booz_gps.h
* \brief Device independent GPS code
*
*/
#ifndef BOOZ_GPS_H
#define BOOZ_GPS_H
#include "std.h"
#define GPS_NB_CHANNELS 16
extern uint8_t gps_mode; /* Receiver status */
extern uint32_t gps_itow; /* ms */
extern int32_t gps_alt; /* cm */
extern uint16_t gps_gspeed; /* cm/s */
extern int16_t gps_climb; /* m/s */
extern int16_t gps_course; /* decideg */
extern int32_t gps_utm_east, gps_utm_north; /** cm */
extern uint8_t gps_utm_zone;
extern int32_t gps_lat, gps_lon; /* 1e7 deg */
extern uint16_t gps_PDOP;
extern uint32_t gps_Pacc, gps_Sacc;
extern uint8_t gps_numSV;
extern uint8_t gps_configuring;
extern uint16_t last_gps_msg_t; /** cputime of the last gps message */
extern bool_t gps_verbose_downlink;
extern volatile uint8_t gps_msg_received;
extern bool_t gps_pos_available;
extern uint8_t gps_nb_ovrn;
/** Number of scanned satellites */
extern uint8_t gps_nb_channels;
/** Space Vehicle Information */
struct svinfo {
uint8_t svid;
uint8_t flags;
uint8_t qi;
uint8_t cno;
int8_t elev; /** deg */
int16_t azim; /** deg */
};
extern struct svinfo gps_svinfos[GPS_NB_CHANNELS];
extern void gps_downlink( void );
#endif /* BOOZ_GPS_H */
sw/airborne/booz/quad_ins.h
-71
View File
@@ -1,71 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2003 Pascal Brisset, 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.
*
*/
/** \file quad_ins.h
* \brief Device independent INS code
*
*/
#ifndef QUAD_INS_H
#define QUAD_INS_H
#include "std.h"
extern volatile uint8_t ins_msg_received;
extern void quad_ins_init( void );
extern void quad_ins_periodic_task( void );
void parse_ins_msg( void );
void parse_ins_buffer( uint8_t );
#ifndef SITL
#include "uart.h"
#define __InsLink(dev, _x) dev##_x
#define _InsLink(dev, _x) __InsLink(dev, _x)
#define InsLink(_x) _InsLink(INS_LINK, _x)
#define InsBuffer() InsLink(ChAvailable())
#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
#define InsUartSend1(c) InsLink(Transmit(c))
#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
#define InsUartRunning InsLink(TxRunning)
#endif /** !SITL */
#define QuadInsEventCheckAndHandle(handler) { \
if (InsBuffer()) { \
ReadInsBuffer(); \
} \
if (ins_msg_received) { \
LED_TOGGLE(2); \
parse_ins_msg(); \
handler(); \
ins_msg_received = FALSE; \
} \
}
#endif /* QUAD_INS_H */
sw/airborne/booz/quad_xsens.c
-374
View File
@@ -1,374 +0,0 @@
/*
* Paparazzi mcu0 $Id$
*
* Copyright (C) 2003 Pascal Brisset, 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.
*
*/
/** \file xsens.c
* \brief Parser for the Xsens protocol
*/
#include "quad_ins.h"
#include <inttypes.h>
#include "airframe.h"
#include "led.h"
#include "booz_estimator.h"
#include "quad_gps.h"
#include "quad_vfilter.h"
#include "agl_vfilter.h"
#include "booz_inter_mcu.h"
#include "booz_autopilot.h"
#include "downlink.h"
#include "messages.h"
#include "latlong.h"
/* comes from booz_inter_mcu.h */
struct booz_inter_mcu_state inter_mcu_state;
volatile uint8_t ins_msg_received;
#define XsensInitCheksum() { send_ck = 0; }
#define XsensUpdateChecksum(c) { send_ck += c; }
#define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }
#define XsensSend1ByAddr(x) { XsensSend1(*x); }
#define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
#define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
#define XsensHeader(msg_id, len) { \
InsUartSend1(XSENS_START); \
XsensInitCheksum(); \
XsensSend1(XSENS_BID); \
XsensSend1(msg_id); \
XsensSend1(len); \
}
#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
/** Includes macros generated from xsens_MTi-G.xml */
#include "xsens_protocol.h"
#define XSENS_MAX_PAYLOAD 254
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
/* output mode : calibrated, orientation, position, velocity, status */
#ifndef XSENS_OUTPUT_MODE
#define XSENS_OUTPUT_MODE 0x0836
#endif
/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED */
#ifndef XSENS_OUTPUT_SETTINGS
#define XSENS_OUTPUT_SETTINGS 0x80000C05
#endif
#ifndef XSENS_NED
#define XSENS_NED 1
#endif
#define UNINIT 0
#define GOT_START 1
#define GOT_BID 2
#define GOT_MID 3
#define GOT_LEN 4
#define GOT_DATA 5
#define GOT_CHECKSUM 6
uint8_t xsens_errorcode;
uint8_t xsens_msg_status;
uint16_t xsens_time_stamp;
uint16_t xsens_output_mode;
uint32_t xsens_output_settings;
static uint8_t xsens_id;
static uint8_t xsens_status;
static uint8_t xsens_len;
static uint8_t xsens_msg_idx;
static uint8_t ck;
uint8_t send_ck;
void quad_ins_init( void ) {
//#ifdef NO_INTERNAL_GPS
// // disconnect the internal GPS
// Configure_GPS_RESET_Pin();
// Set_GPS_RESET_Pin_LOW();
//#endif
xsens_status = UNINIT;
xsens_msg_status = 0;
xsens_time_stamp = 0;
xsens_output_mode = XSENS_OUTPUT_MODE;
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
//XSENS_GoToMeasurment();
}
void quad_ins_periodic_task( void ) {
static uint8_t _1hz = 0;
_1hz++;
if (_1hz >= 100) _1hz = 0;
switch (_1hz) {
case 0:
//XSENS_GoToConfig();
XSENS_ReqGPSStatus();
//XSENS_GoToMeasurment();
break;
}
}
void parse_ins_msg( void ) {
uint8_t offset = 0;
if (xsens_id == XSENS_ReqOutputModeAck_ID) {
xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
}
else if (xsens_id == XSENS_ReqOutputSettings_ID) {
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
}
else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
}
else if (xsens_id == XSENS_GPSStatus_ID) {
//DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf);
gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
uint8_t i;
for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
if (ch > GPS_NB_CHANNELS) continue;
gps_svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps_svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
//DOWNLINK_SEND_SVINFO(&ch, &gps_svinfos[ch].svid, &gps_svinfos[ch].flags, &gps_svinfos[ch].qi, &gps_svinfos[ch].cno,&gps_svinfos[ch].elev, &gps_svinfos[ch].azim);
}
}
else if (xsens_id == XSENS_MTData_ID) {
//uint8_t offset = 0;
/* test RAW modes else calibrated modes */
//if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
booz_estimator_uf_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
booz_estimator_p = booz_estimator_uf_p;
booz_estimator_uf_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
booz_estimator_q = booz_estimator_uf_q;
booz_estimator_uf_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
booz_estimator_r = booz_estimator_uf_r;
//booz_autopilot_mode = BOOZ_AP_MODE_RATE;
offset += XSENS_DATA_RAWInertial_LENGTH;
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset);
gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
#ifdef NAV_HORIZONTAL
/* Set the real UTM zone */
gps_utm_zone = (gps_lon/1e7+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), gps_utm_zone);
/* utm */
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
booz_estimator_x = latlong_utm_x;
booz_estimator_y = latlong_utm_y;
#endif
gps_alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 10;
booz_estimator_z = -(float)gps_alt / 100.;
booz_estimator_vx = (float)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
booz_estimator_vy = (float)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
booz_estimator_vz = (float)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset);
gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset);
offset += XSENS_DATA_RAWGPS_LENGTH;
}
//} else {
if (XSENS_MASK_Temp(xsens_output_mode)) {
offset += XSENS_DATA_Temp_LENGTH;
}
if (XSENS_MASK_Calibrated(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_AccOut(xsens_output_settings)) {
inter_mcu_state.accel[AXIS_X] = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
inter_mcu_state.accel[AXIS_Y] = XSENS_NED * XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
inter_mcu_state.accel[AXIS_Z] = XSENS_NED * XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
#ifdef USE_VFILTER
#ifdef USE_BARO_MS5534A
inter_mcu_state.pos[AXIS_Z] = quad_vf_z;
inter_mcu_state.speed[AXIS_Z] = quad_vf_zdot;
gps_alt = inter_mcu_state.pos[AXIS_Z] * 100;
gps_climb = (int16_t)(inter_mcu_state.speed[AXIS_Z] * 100);
#endif
#ifdef USE_TELEMETER
booz_estimator_agl_z = agl_vf_z;
booz_estimator_agl_zdot = agl_vf_zdot;
#endif
#endif
l++;
}
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
inter_mcu_state.r_rates[AXIS_P] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset) * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_Q] = XSENS_NED * XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset) * RATE_PI_S/M_PI;
inter_mcu_state.r_rates[AXIS_R] = XSENS_NED * XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset) * RATE_PI_S/M_PI;
inter_mcu_state.f_rates[AXIS_P] = inter_mcu_state.r_rates[AXIS_P];
inter_mcu_state.f_rates[AXIS_Q] = inter_mcu_state.r_rates[AXIS_Q];
inter_mcu_state.f_rates[AXIS_R] = inter_mcu_state.r_rates[AXIS_R];
l++;
}
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
l++;
}
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
}
if (XSENS_MASK_Orientation(xsens_output_mode)) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
float dcm01 = 2 * (q1*q2 + q0*q3);
float dcm02 = 2 * (q1*q3 - q0*q2);
float dcm12 = 2 * (q2*q3 + q0*q1);
float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
inter_mcu_state.f_eulers[AXIS_X] = atan2(dcm12, dcm22) * ANGLE_PI/M_PI;
inter_mcu_state.f_eulers[AXIS_Y] = -asin(dcm02) * ANGLE_PI/M_PI;
inter_mcu_state.f_eulers[AXIS_Z] = atan2(dcm01, dcm00) * ANGLE_PI/M_PI;
gps_course = inter_mcu_state.f_eulers[AXIS_Z] * 1800 / ANGLE_PI;
offset += XSENS_DATA_Quaternion_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
inter_mcu_state.f_eulers[AXIS_X] = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * ANGLE_PI/180;
inter_mcu_state.f_eulers[AXIS_Y] = XSENS_NED * XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * ANGLE_PI/180;
inter_mcu_state.f_eulers[AXIS_Z] = XSENS_NED * XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * ANGLE_PI/180;
gps_course = (int16_t)(XSENS_NED * XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * 10);
offset += XSENS_DATA_Euler_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
l++;
}
if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
l++;
}
offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
}
if (XSENS_MASK_Position(xsens_output_mode)) {
//DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf);
float lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset);
float lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset);
gps_lat = (int32_t)(lat * 1e7);
gps_lon = (int32_t)(lon * 1e7);
//#ifdef NAV_HORIZONTAL
gps_utm_zone = (lon+180) / 6 + 1;
latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), gps_utm_zone);
//#endif
inter_mcu_state.pos[AXIS_X] = latlong_utm_x;
inter_mcu_state.pos[AXIS_Y] = latlong_utm_y;
gps_utm_east = inter_mcu_state.pos[AXIS_X] * 100;
gps_utm_north = inter_mcu_state.pos[AXIS_Y] * 100;
#ifndef USE_VFILTER
inter_mcu_state.pos[AXIS_Z] = -XSENS_DATA_Position_alt(xsens_msg_buf,offset);
gps_alt = inter_mcu_state.pos[AXIS_Z] * 100;
#endif
offset += XSENS_DATA_Position_LENGTH;
}
if (XSENS_MASK_Velocity(xsens_output_mode)) {
//DOWNLINK_SEND_DEBUG(xsens_len,xsens_msg_buf);
inter_mcu_state.speed[AXIS_X] = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
inter_mcu_state.speed[AXIS_Y] = XSENS_NED * XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
#ifndef USE_VFILTER
inter_mcu_state.speed[AXIS_Z] = XSENS_NED * XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
gps_climb = (int16_t)(-inter_mcu_state.speed[AXIS_Z] * 100);
#endif
gps_gspeed = (uint16_t)(sqrt(inter_mcu_state.speed[AXIS_X]*inter_mcu_state.speed[AXIS_X] + inter_mcu_state.speed[AXIS_Y]*inter_mcu_state.speed[AXIS_Y]) * 100);
offset += XSENS_DATA_Velocity_LENGTH;
}
if (XSENS_MASK_Status(xsens_output_mode)) {
xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
if (bit_is_set(xsens_msg_status,2)) gps_mode = 0x03; // gps fix
else gps_mode = 0;
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
gps_itow = xsens_time_stamp;
offset += XSENS_DATA_TimeStamp_LENGTH;
}
//}
}
//DOWNLINK_SEND_QUAD_INS(&offset,&xsens_len,&xsens_msg_status,&xsens_time_stamp);
}
void parse_ins_buffer( uint8_t c ) {
ck += c;
switch (xsens_status) {
case UNINIT:
if (c != XSENS_START)
goto error;
xsens_status++;
ck = 0;
break;
case GOT_START:
if (c != XSENS_BID)
goto error;
xsens_status++;
break;
case GOT_BID:
xsens_id = c;
xsens_status++;
break;
case GOT_MID:
xsens_len = c;
if (xsens_len > XSENS_MAX_PAYLOAD)
goto error;
xsens_msg_idx = 0;
xsens_status++;
break;
case GOT_LEN:
xsens_msg_buf[xsens_msg_idx] = c;
xsens_msg_idx++;
if (xsens_msg_idx >= xsens_len)
xsens_status++;
break;
case GOT_DATA:
if (ck != 0)
goto error;
ins_msg_received = TRUE;
goto restart;
break;
}
return;
error:
restart:
xsens_status = UNINIT;
return;
}