Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

updated sim for new supervision code

Browse Source
This commit is contained in:
Antoine Drouin
2009-08-15 18:22:42 +00:00
parent e4fcd0eba5
commit 56acfeda19
6 changed files with 29 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/nps/nps_autopilot_booz.c
+8 -4
View File
@@ -7,7 +7,7 @@
#include "booz_imu.h"
#include "booz2_analog_baro.h"
#include "actuators.h"
#include "actuators/booz_supervision.h"
struct NpsAutopilot autopilot;
@@ -79,7 +79,11 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
autopilot.commands[SERVO_LEFT] = hover - yaw + roll;
}
else {
int32_t ut_front = Actuator(SERVO_FRONT) - TRIM_FRONT;
uint8_t i;
for (i=0; i<ACTUATORS_MKK_NB; i++)
autopilot.commands[i] = (double)supervision_commands[i] / SUPERVISION_MAX_MOTOR;
#if 0
int32_t ut_front = supervision_commands[SERVO_FRONT] - TRIM_FRONT;
int32_t ut_back = Actuator(SERVO_BACK) - TRIM_BACK;
int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT;
int32_t ut_left = Actuator(SERVO_LEFT) - TRIM_LEFT;
@@ -87,10 +91,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
autopilot.commands[SERVO_BACK] = (double)ut_back / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_RIGHT] = (double)ut_right / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_LEFT] = (double)ut_left / SUPERVISION_MAX_MOTOR;
#endif
}
// printf("%f %f %f %f\n", autopilot.commands[SERVO_FRONT], autopilot.commands[SERVO_BACK],
// autopilot.commands[SERVO_RIGHT], autopilot.commands[SERVO_LEFT]);
// autopilot.commands[SERVO_RIGHT], autopilot.commands[SERVO_LEFT]);
}
#include "nps_fdm.h"
sw/simulator/nps/nps_ivy.c
+7 -4
View File
@@ -57,11 +57,14 @@ void nps_ivy_display(void) {
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.psi));
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.ltp_ecef_vel.x),
(fdm.ltp_ecef_vel.y),
(fdm.ltp_ecef_vel.z),
(fdm.ltpprz_ecef_accel.x),
(fdm.ltpprz_ecef_accel.y),
(fdm.ltpprz_ecef_accel.z),
(fdm.ltpprz_ecef_vel.x),
(fdm.ltpprz_ecef_vel.y),
(fdm.ltpprz_ecef_vel.z),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.z));
sw/simulator/nps/nps_main.c
+1 -1
View File
@@ -13,7 +13,7 @@
#include "nps_flightgear.h"
#define SIM_DT (1./512.)
#define DISPLAY_DT (1./25.)
#define DISPLAY_DT (1./30.)
#define HOST_TIMEOUT_MS 40
#define HOST_TIME_FACTOR 1.
sw/simulator/nps/nps_radio_control.c
+11 -1
View File
@@ -35,12 +35,14 @@ void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char*
typedef void (*rc_script)(double);
static void radio_control_script_takeoff(double time);
static void radio_control_script_hover(double time);
static void radio_control_script_step_roll(double time);
static void radio_control_script_step_pitch(double time);
static void radio_control_script_step_yaw(double time);
static void radio_control_script_ff(double time);
static rc_script scripts[] = {
radio_control_script_hover,
radio_control_script_step_roll,
radio_control_script_step_pitch,
radio_control_script_step_yaw,
@@ -91,6 +93,14 @@ void radio_control_script_takeoff(double time) {
}
void radio_control_script_hover(double time __attribute__ ((unused))) {
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
nps_radio_control.roll = 0.;
nps_radio_control.yaw = 0.;
}
void radio_control_script_step_roll(double time) {
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
@@ -128,7 +138,7 @@ void radio_control_script_step_yaw(double time) {
nps_radio_control.yaw = 0.5;
}
else {
nps_radio_control.yaw = 0.;
nps_radio_control.yaw = -0.5;
}
}
sw/simulator/nps/nps_sensor_accel.c
+2 -1
View File
@@ -38,7 +38,7 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
/* substract gravity to acceleration in body frame */
struct DoubleVect3 accelero_body;
FLOAT_VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
// printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z);
@@ -49,6 +49,7 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
/* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
VECT3_ADD(accel->value, accel->neutral);
/* Compute sensor error */
struct DoubleVect3 accelero_error;
/* constant bias */
sw/simulator/nps_autopilot_booz.c
-125
View File
@@ -1,125 +0,0 @@
#include "nps_autopilot.h"
#include "booz2_main.h"
#include "nps_sensors.h"
#include "nps_radio_control.h"
#include "booz_radio_control.h"
#include "booz_imu.h"
#include "booz2_analog_baro.h"
#include "actuators.h"
#define BYPASS_AHRS
struct NpsAutopilot autopilot;
#ifdef BYPASS_AHRS
static void sim_overwrite_ahrs(void);
#endif
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* js_dev) {
nps_radio_control_init(type_rc, num_rc_script, js_dev);
booz2_main_init();
}
#include <stdio.h>
#include "booz2_gps.h"
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
if (nps_radio_control_available(time)) {
booz_radio_control_feed();
booz2_main_event();
}
if (nps_sensors_gyro_available()) {
booz_imu_feed_gyro_accel();
booz2_main_event();
}
if (nps_sensors_mag_available()) {
booz_imu_feed_mag();
booz2_main_event();
}
if (nps_sensors_baro_available()) {
Booz2BaroISRHandler(sensors.baro.value);
booz2_main_event();
}
#ifdef BYPASS_AHRS
sim_overwrite_ahrs();
#endif /* BYPASS_AHRS */
if (nps_sensors_gps_available()) {
booz_gps_state.ecef_pos.x = sensors.gps.ecef_pos.x*100.;
booz_gps_state.ecef_pos.y = sensors.gps.ecef_pos.y*100.;
booz_gps_state.ecef_pos.z = sensors.gps.ecef_pos.z*100.;
booz_gps_state.ecef_vel.x = sensors.gps.ecef_vel.x*100.;
booz_gps_state.ecef_vel.y = sensors.gps.ecef_vel.y*100.;
booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z*100.;
booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
// booz2_main_event();
}
booz2_main_periodic();
/* 25 */
if (time < 8) {
// double hover = 0.25;
double hover = 0.2493;
// double hover = 0.23;
// double hover = 0.;
// if (time > 20) hover = 0.25;
double yaw = 0.000000;
double pitch = 0.000;
double roll = 0.0000;
autopilot.commands[SERVO_FRONT] = hover + yaw + pitch;
autopilot.commands[SERVO_BACK] = hover + yaw - pitch;
autopilot.commands[SERVO_RIGHT] = hover - yaw - roll ;
autopilot.commands[SERVO_LEFT] = hover - yaw + roll;
}
else {
int32_t ut_front = Actuator(SERVO_FRONT) - TRIM_FRONT;
int32_t ut_back = Actuator(SERVO_BACK) - TRIM_BACK;
int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT;
int32_t ut_left = Actuator(SERVO_LEFT) - TRIM_LEFT;
autopilot.commands[SERVO_FRONT] = (double)ut_front / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_BACK] = (double)ut_back / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_RIGHT] = (double)ut_right / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_LEFT] = (double)ut_left / SUPERVISION_MAX_MOTOR;
}
// printf("%f %f %f %f\n", autopilot.commands[SERVO_FRONT], autopilot.commands[SERVO_BACK],
// autopilot.commands[SERVO_RIGHT], autopilot.commands[SERVO_LEFT]);
}
#ifdef BYPASS_AHRS
#include "nps_fdm.h"
#include "math/pprz_algebra_int.h"
#include "booz_ahrs.h"
static void sim_overwrite_ahrs(void) {
// printf("%f\n", fdm.ltpprz_to_body_eulers.phi);
booz_ahrs.ltp_to_body_euler.phi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.phi);
booz_ahrs.ltp_to_body_euler.theta = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.theta);
booz_ahrs.ltp_to_body_euler.psi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.psi);
booz_ahrs.ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qi);
booz_ahrs.ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qx);
booz_ahrs.ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qy);
booz_ahrs.ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qz);
booz_ahrs.body_rate.p = RATE_BFP_OF_REAL(fdm.body_ecef_rotvel.p);
booz_ahrs.body_rate.q = RATE_BFP_OF_REAL(fdm.body_ecef_rotvel.q);
booz_ahrs.body_rate.r = RATE_BFP_OF_REAL(fdm.body_ecef_rotvel.r);
}
#endif /* BYPASS_AHRS */