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

[ahrs] update remaining ahrs stuff

Browse Source
This commit is contained in:
Felix Ruess
2015-01-16 17:28:43 +01:00
parent ca464e37a2
commit e8481df5eb
11 changed files with 43 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+1 -1
View File
@@ -157,7 +157,7 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) { mde = 2; }
if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
if (imu_lost) { mde = 5; }
uint16_t val = imu_lost_counter;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
sw/airborne/modules/datalink/mavlink.c
+1 -1
View File
@@ -296,7 +296,7 @@ static inline void mavlink_send_heartbeat(void)
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
#endif
if (ahrs.status == AHRS_RUNNING) {
if (DefaultAhrsImpl.is_aligned) {
if (kill_throttle) {
mav_state = MAV_STATE_STANDBY;
}
sw/airborne/modules/ins/ahrs_chimu.h
+7 -1
View File
@@ -29,9 +29,15 @@
#include "modules/ins/ins_module.h"
#include "subsystems/ahrs.h"
struct AhrsChimu {
bool_t is_aligned;
};
extern struct AhrsChimu ahrs_chimu;
#define DefaultAhrsImpl ahrs_chimu
extern void ahrs_chimu_register(void);
extern void ahrs_chimu_init(struct OrientationReps* body_to_imu);
extern void ahrs_chimu_init(void);
#endif
sw/airborne/modules/ins/ahrs_chimu_spi.c
+5 -3
View File
@@ -35,6 +35,8 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
struct AhrsChimu ahrs_chimu;
void ahrs_chimu_update_gps(void);
void ahrs_chimu_register(void)
@@ -42,9 +44,9 @@ void ahrs_chimu_register(void)
ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
void ahrs_chimu_init(void)
{
ahrs.status = AHRS_UNINIT;
ahrs_chimu.is_aligned = FALSE;
// uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
@@ -103,7 +105,7 @@ void parse_ins_msg(void)
}; // FIXME rate r
stateSetBodyRates_f(&rates);
//FIXME
ahrs.status = AHRS_RUNNING;
ahrs_chimu.is_aligned = TRUE;
} else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
sw/airborne/modules/ins/ahrs_chimu_uart.c
+5 -3
View File
@@ -30,14 +30,16 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
struct AhrsChimu ahrs_chimu;
void ahrs_chimu_register(void)
{
ahrs_register_impl(ahrs_chimu_init, NULL);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
void ahrs_chimu_init(void)
{
ahrs.status = AHRS_UNINIT;
ahrs_chimu.is_aligned = FALSE;
uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
@@ -87,7 +89,7 @@ void parse_ins_msg(void)
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
ahrs.status = AHRS_RUNNING;
ahrs_chimu.is_aligned = TRUE;
#if CHIMU_DOWNLINK_IMMEDIATE
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi,
&CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+4 -2
View File
@@ -32,6 +32,7 @@
# include <stdio.h>
#endif
#include "subsystems/ahrs.h"
#include "ahrs_ardrone2.h"
#include "state.h"
#include "math/pprz_algebra_float.h"
@@ -71,14 +72,15 @@ void ahrs_ardrone2_register(void)
ahrs_register_impl(ahrs_ardrone2_init, NULL);
}
void ahrs_ardrone2_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
void ahrs_ardrone2_init(void)
{
init_at_com();
//Set navdata_demo to FALSE and flat trim the ar drone
at_com_send_config("general:navdata_demo", "FALSE");
at_com_send_ftrim();
ahrs.status = AHRS_RUNNING;
ahrs_ardrone2.is_aligned = TRUE;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
+2 -2
View File
@@ -30,7 +30,6 @@
#ifndef AHRS_ARDRONE2_H
#define AHRS_ARDRONE2_H
#include "subsystems/ahrs.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_geodetic_float.h"
@@ -44,13 +43,14 @@ struct AhrsARDrone {
int32_t altitude; // in cm above ground
uint32_t battery; // in percentage
struct Int32Quat ltp_to_imu_quat;
bool_t is_aligned;
};
extern struct AhrsARDrone ahrs_ardrone2;
#define DefaultAhrsImpl ahrs_ardrone2
extern void ahrs_ardrone2_register(void);
extern void ahrs_ardrone2_init(struct OrientationReps* body_to_imu);
extern void ahrs_ardrone2_init(void);
extern void ahrs_ardrone2_propagate(void);
#endif /* AHRS_ARDRONE2_H */
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+5 -2
View File
@@ -38,6 +38,8 @@
#include "state.h"
#include "subsystems/abi.h"
struct AhrsInfrared ahrs_infrared;
float heading;
/** ABI binding for gyro data.
@@ -77,8 +79,9 @@ void ahrs_infrared_register(void)
ahrs_register_impl(ahrs_infrared_init, ahrs_infrared_update_gps);
}
void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
ahrs.status = AHRS_RUNNING;
void ahrs_infrared_init(void)
{
ahrs_infrared.is_aligned = TRUE;
heading = 0.;
sw/airborne/subsystems/ahrs/ahrs_infrared.h
+7 -1
View File
@@ -33,8 +33,14 @@
#include "subsystems/ahrs.h"
#include "math/pprz_orientation_conversion.h"
struct AhrsInfrared {
bool_t is_aligned;
};
extern struct AhrsInfrared ahrs_infrared;
extern void ahrs_infrared_register(void);
extern void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused)));
extern void ahrs_infrared_init(void);
extern void ahrs_update_infrared(void);
extern void ahrs_infrared_update_gps(void);
sw/airborne/subsystems/ahrs/ahrs_sim.c
+4 -6
View File
@@ -26,7 +26,6 @@
*
*/
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_sim.h"
#include "math/pprz_algebra_float.h"
#include "generated/airframe.h"
@@ -50,7 +49,8 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
void update_ahrs_from_sim(void) {
void update_ahrs_from_sim(void)
{
struct FloatEulers ltp_to_imu_euler = { sim_phi, sim_theta, sim_psi };
struct FloatRates imu_rate = { sim_p, sim_q, sim_r };
@@ -61,8 +61,6 @@ void update_ahrs_from_sim(void) {
}
void ahrs_sim_init(void) {
//ahrs_float.status = AHRS_UNINIT;
// set to running for now
ahrs.status = AHRS_RUNNING;
void ahrs_sim_init(void)
{
}
sw/airborne/test/subsystems/test_ahrs.c
+2 -4
View File
@@ -110,11 +110,9 @@ static inline void on_gyro_event(void)
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
if (ahrs.status != AHRS_RUNNING) {
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
ahrs.status = AHRS_RUNNING;
}
return;
}
#endif
}