STATIC_INLINE void main_periodic(void)
{
#if USE_IMU
imu_periodic();
#endif
//FIXME: temporary hack, remove me
#ifdef InsPeriodic
InsPeriodic();
#endif
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
SetActuatorsFromCommands(commands, autopilot_mode);
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
STATIC_INLINE void main_periodic(void)
{
#if INTER_MCU_AP
/* Inter-MCU watchdog */
intermcu_periodic();
#endif
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
#ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_mode);
#else
intermcu_set_actuators(commands, autopilot_mode);
#endif
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
}
else if(CHIMU_DATA.m_MsgID==0x02)
{
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
}
}
}
}
void autopilot_periodic(void)
{
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
if (dist2_to_home > failsafe_mode_dist2) {
autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_set_mode(AP_MODE_HOME);
}
}
}
if (autopilot_mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
} else {
// otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight) {
autopilot_set_mode(AP_MODE_KILL);
}
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected) {
autopilot_set_mode(AP_MODE_KILL);
}
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight) {
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
} else {
guidance_v_run(autopilot_in_flight);
guidance_h_run(autopilot_in_flight);
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
void periodic_task_fbw(void) {
/* static float t; */
/* t += 1./60.; */
/* uint16_t servo_value = 1500+ 500*sin(t); */
/* SetServo(SERVO_THROTTLE, servo_value); */
RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, SERVOS_NB, actuators ));
}
void autopilot_periodic(void) {
if (autopilot_in_flight) {
if (too_far_from_home) {
if (dist2_to_home > failsafe_mode_dist2)
autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
else
autopilot_set_mode(AP_MODE_HOME);
}
}
if (autopilot_mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
}
else {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight)
autopilot_set_mode(AP_MODE_KILL);
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected)
autopilot_set_mode(AP_MODE_KILL);
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight || autopilot_ground_detected) {
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
void ArduIMU_event(void)
{
// Handle INS I2C event
if (ardu_ins_trans.status == I2CTransSuccess) {
// received data from I2C transaction
recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0];
recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2];
recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4];
recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6];
recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8];
recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10];
recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12];
recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14];
recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral;
arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral;
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
stateSetNedToBodyEulers_f(&arduimu_eulers);
stateSetBodyRates_f(&arduimu_rates);
stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel));
ardu_ins_trans.status = I2CTransDone;
#ifdef ARDUIMU_SYNC_SEND
// uint8_t arduimu_id = 102;
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q,
&arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y,
&arduimu_accel.z));
#endif
} else if (ardu_ins_trans.status == I2CTransFailed) {
ardu_ins_trans.status = I2CTransDone;
}
// Handle GPS I2C event
if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
ardu_gps_trans.status = I2CTransDone;
}
}
void actuators_mkk_v2_set(void)
{
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; }
else { actuators_delay_done = TRUE; }
}
#endif
// Read result
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
if (actuators_mkk_v2.trans[i].type != I2CTransTx) {
actuators_mkk_v2.trans[i].type = I2CTransTx;
actuators_mkk_v2.data[i].Current = actuators_mkk_v2.trans[i].buf[0];
actuators_mkk_v2.data[i].MaxPWM = actuators_mkk_v2.trans[i].buf[1];
actuators_mkk_v2.data[i].Temperature = actuators_mkk_v2.trans[i].buf[2];
}
}
RunOnceEvery(10, actuators_mkk_v2_read());
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
#ifdef KILL_MOTORS
actuators_mkk_v2.trans[i].buf[0] = 0;
actuators_mkk_v2.trans[i].buf[1] = 0;
#else
actuators_mkk_v2.trans[i].buf[0] = (actuators_mkk_v2.setpoint[i] >> 3);
actuators_mkk_v2.trans[i].buf[1] = actuators_mkk_v2.setpoint[i] & 0x07;
#endif
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &actuators_mkk_v2.trans[i]);
}
}
void atmega_i2c_cam_ctrl_send(uint8_t cmd)
{
static uint8_t zoom = 0;
static uint8_t mode = 0;
unsigned char cam_ret[1];
if (cmd == DC_SHOOT)
{
dc_send_shot_position();
}
else if (cmd == DC_TALLER)
{
zoom = 1;
}
else if (cmd == DC_WIDER)
{
zoom = 0;
}
else if (cmd == DC_GET_STATUS)
{
mode++;
if (mode > 15)
mode = 0;
}
cam_ret[0] = mode + zoom * 0x20;
RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret ));
}
void imu_periodic(void)
{
mpu60x0_spi_periodic(&imu_px4fmu.mpu);
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
RunOnceEvery(10, hmc58xx_periodic(&imu_px4fmu.hmc));
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#ifdef FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
if ( !autopilot_motors_on ||
#ifndef FAILSAFE_GROUND_DETECT
autopilot_mode == AP_MODE_FAILSAFE ||
#endif
autopilot_mode == AP_MODE_KILL ) {
SetCommands(commands_failsafe,
autopilot_in_flight, autopilot_motors_on);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetCommands(stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
#ifdef AUTOPILOT_LOBATT_WING_WAGGLE
if (electrical.vsupply < (MIN_BAT_LEVEL * 10)){
RunOnceEvery(autopilot_lobatt_wing_waggle_interval,{setpoint_lobatt_wing_waggle_num=0;})
}
void baro_bmp_event(void) {
bmp085_event(&baro_bmp);
if (baro_bmp.data_available) {
float tmp = baro_bmp.pressure / 101325.0; // pressure at sea level
tmp = pow(tmp, 0.190295);
baro_bmp_alt = 44330 * (1.0 - tmp);
float pressure = (float)baro_bmp.pressure;
AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, &pressure);
baro_bmp.data_available = FALSE;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up,
&baro_bmp.ut, &baro_bmp.pressure,
&baro_bmp.temperature);
#else
RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice,
&baro_bmp.up, &baro_bmp.ut,
&baro_bmp.pressure,
&baro_bmp.temperature));
#endif
}
}
void ins_periodic_task( void ) {
if (xsens_configured > 0)
{
switch (xsens_configured)
{
case 20:
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
break;
case 18:
// Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002);
break;
case 17:
// 1 pulse every 100 samples
XSENS_SetSyncOutSettings(1,100);
break;
case 2:
XSENS_ReqLeverArmGps();
break;
case 6:
XSENS_ReqMagneticDeclination();
break;
case 13:
#ifdef AHRS_H_X
#pragma message "Sending XSens Magnetic Declination."
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
XSENS_SetMagneticDeclination(xsens_declination);
#endif
break;
case 12:
#ifdef GPS_IMU_LEVER_ARM_X
#pragma message "Sending XSens GPS Arm."
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
#endif
break;
case 10:
{
uint8_t baud = 1;
XSENS_SetBaudrate(baud);
}
break;
case 1:
XSENS_GoToMeasurment();
break;
default:
break;
}
xsens_configured--;
return;
}
RunOnceEvery(100,XSENS_ReqGPSStatus());
}
void parse_ins_msg(void)
{
struct link_device *dev = InsLinkDevice;
while (dev->char_available(dev->periph)) {
uint8_t ch = dev->get_byte(dev->periph);
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
if (CHIMU_DATA.m_MsgID == 0x03) {
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
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);
#endif
}
}
}
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#ifdef FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
if ( !autopilot_motors_on ||
#ifndef FAILSAFE_GROUND_DETECT
autopilot_mode == AP_MODE_FAILSAFE ||
#endif
autopilot_mode == AP_MODE_KILL ) {
SetCommands(commands_failsafe,
autopilot_in_flight, autopilot_motors_on);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetCommands(stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
}
void humid_sht_periodic( void ) {
switch (sht_status) {
case SHT_UNINIT:
/* do soft reset, then wait at least 15ms */
sht_status = SHT_RESET;
sht_trans.buf[0] = SHT_SOFT_RESET;
I2CTransmit(SHT_I2C_DEV, sht_trans, SHT_SLAVE_ADDR, 1);
break;
case SHT_SERIAL:
/* get serial number part 1 */
sht_status = SHT_SERIAL1;
sht_trans.buf[0] = 0xFA;
sht_trans.buf[1] = 0x0F;
I2CTransceive(SHT_I2C_DEV, sht_trans, SHT_SLAVE_ADDR, 2, 8);
break;
case SHT_SERIAL1:
case SHT_SERIAL2:
break;
default:
/* trigger temp measurement, no master hold */
sht_trans.buf[0] = SHT_TRIGGER_TEMP;
sht_status = SHT_TRIG_TEMP;
I2CTransmit(SHT_I2C_DEV, sht_trans, SHT_SLAVE_ADDR, 1);
/* send serial number every 30 seconds */
RunOnceEvery((4*30), DOWNLINK_SEND_SHT_SERIAL(DefaultChannel, &sht_serial1, &sht_serial2));
break;
}
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS;
i2c_transceive(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 1, 21);
/*
// Start reading the latest accelerometer data
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6);
*/
// Start reading the latest magnetometer data
#if PERIODIC_FREQUENCY > 60
RunOnceEvery(2,{
#endif
/* ppzuavimu_hmc5843.type = I2CTransTxRx;
ppzuavimu_hmc5843.len_r = 6;
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843);
*/
#if PERIODIC_FREQUENCY > 60
});
#endif
//RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
}
void baro_event(void)
{
if (sys_time.nb_sec > 1) {
ms5611_spi_event(&bb_ms5611);
if (bb_ms5611.data_available) {
float pressure = (float)bb_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = false;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
#endif
#if DEBUG
float fbaroms = bb_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&bb_ms5611.data.d1, &bb_ms5611.data.d2,
&fbaroms, &temp);
#endif
}
}
}
/*
F = [ 1 dt -dt^2/2 0
0 1 -dt 0
0 0 1 0
0 0 0 1 ];
B = [ dt^2/2 dt 0 0]';
Q = [ Qzz 0 0 0
0 Qzdotzdot 0 0
0 0 Qbiasbias 0
0 0 0 0 Qoffoff ];
Qzz = ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
Qzdotzdot = ACCEL_NOISE * DT_VFILTER
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel, float dt)
{
/* update state */
vff.zdotdot = accel + 9.81 - vff.bias;
vff.z = vff.z + dt * vff.zdot;
vff.zdot = vff.zdot + dt * vff.zdotdot;
/* update covariance */
const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
vff.P[0][0] = FPF00 + ACCEL_NOISE * dt * dt / 2.;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + ACCEL_NOISE * dt;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
vff.P[2][2] = FPF22 + Qbiasbias;
vff.P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
#endif
}
void ahrs_dcm_update_mag(struct Int32Vect3 *mag)
{
#if USE_MAGNETOMETER
#warning MAGNETOMETER FEEDBACK NOT TESTED YET
MAG_FLOAT_OF_BFP(mag_float, *mag);
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
// Pitch&Roll Compensation:
MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;
/*
*
// Magnetic Heading
Heading = atan2(-Head_Y,Head_X);
// Declination correction (if supplied)
if( declination != 0.0 )
{
Heading = Heading + declination;
if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
Heading -= (2.0 * M_PI);
else if (Heading < -M_PI)
Heading += (2.0 * M_PI);
}
// Optimization for external DCM use. Calculate normalized components
Heading_X = cos(Heading);
Heading_Y = sin(Heading);
*/
struct FloatVect3 ltp_mag;
ltp_mag.x = MAG_Heading_X;
ltp_mag.y = MAG_Heading_Y;
#if FLOAT_DCM_SEND_DEBUG
// Downlink
RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, <p_mag.x, <p_mag.y, <p_mag.z));
#endif
// Magnetic Heading
// MAG_Heading = atan2(mag->y, -mag->x);
#else // !USE_MAGNETOMETER
// get rid of unused param warning...
mag = mag;
#endif
}
void baro_hca_read_event( void ) {
pBaroRaw = 0;
// Get raw altimeter from buffer
pBaroRaw = ((uint16_t)baro_hca_i2c_trans.buf[0] << 8) | baro_hca_i2c_trans.buf[1];
if (pBaroRaw == 0)
baro_hca_valid = FALSE;
else
baro_hca_valid = TRUE;
if (baro_hca_valid) {
//Cut RAW Min and Max
if (pBaroRaw < BARO_HCA_MIN_OUT)
pBaroRaw = BARO_HCA_MIN_OUT;
if (pBaroRaw > BARO_HCA_MAX_OUT)
pBaroRaw = BARO_HCA_MAX_OUT;
}
baro_hca_i2c_trans.status = I2CTransDone;
uint16_t foo = 0;
float bar = 0;
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar)
#else
RunOnceEvery(10, DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar));
#endif
}
/*
F = [ 1 dt -dt^2/2 0
0 1 -dt 0
0 0 1 0
0 0 0 1 ];
B = [ dt^2/2 dt 0 0]';
Q = [ Qzz 0 0 0
0 Qzdotzdot 0 0
0 0 Qbiasbias 0
0 0 0 0 Qoffoff ];
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel) {
/* update state */
vff_zdotdot = accel + 9.81 - vff_bias;
vff_z = vff_z + DT_VFILTER * vff_zdot;
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
/* update covariance */
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
const float FPF22 = vff_P[2][2];
const float FPF33 = vff_P[3][3];
vff_P[0][0] = FPF00 + Qzz;
vff_P[0][1] = FPF01;
vff_P[0][2] = FPF02;
vff_P[1][0] = FPF10;
vff_P[1][1] = FPF11 + Qzdotzdot;
vff_P[1][2] = FPF12;
vff_P[2][0] = FPF20;
vff_P[2][1] = FPF21;
vff_P[2][2] = FPF22 + Qbiasbias;
vff_P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe());
#endif
}
void aoa_pwm_update(void) {
static float prev_aoa = 0.0f;
// raw duty cycle in usec
uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
// remove some offset if needed
aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
// FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
// this case is not handled since we don't care about angles close to +- 180 deg
aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
// filter angle
aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
prev_aoa = aoa_pwm.angle;
#if USE_AOA
stateSetAngleOfAttack_f(aoa_adc.angle);
#endif
#if SEND_SYNC_AOA
RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
#endif
#if LOG_AOA
if(pprzLogFile != -1) {
if (!log_started) {
sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
log_started = true;
} else {
float angle = DegOfRad(aoa_pwm.angle);
sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
}
}
#endif
}
/**
* Handle all the periodic tasks of the Navstik IMU components.
* Read the MPU60x0 every periodic call and the HMC58XX every 10th call.
*/
void imu_navstik_periodic(void)
{
// Start reading the latest gyroscope data
mpu60x0_i2c_periodic(&imu_navstik.mpu);
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
RunOnceEvery(10, hmc58xx_periodic(&imu_navstik.hmc));
}
void hmc5843_module_periodic ( void )
{
hmc5843_periodic();
mag_x = hmc5843.data.value[0];
mag_y = hmc5843.data.value[1];
mag_z = hmc5843.data.value[2];
RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&mag_x,&mag_y,&mag_z));
}
static inline void main_periodic_task(void)
{
RunOnceEvery(100, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
if (sys_time.nb_sec > 1) {
lis302dl_spi_periodic(&lis302);
#if USE_LED_5
RunOnceEvery(10, LED_TOGGLE(5););
void event_i2c_abuse_test(void)
{
if (i2c_idle(&I2C_ABUSE_PORT))
{
LED_ON(5); // green = idle
LED_OFF(4);
}
else
{
LED_ON(4); // red = busy
LED_OFF(5);
}
// Wait for I2C transaction object to be released by the I2C driver before changing anything
if ((i2c_abuse_test_counter < 12) && (i2c_abuse_test_counter > 3))
{
if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess))
{
//i2c_test2.slave_addr = 0x90;
i2c_test2.type = I2CTransRx;
i2c_test2.slave_addr = 0x92;
i2c_test2.len_r = 2;
i2c_submit(&I2C_ABUSE_PORT,&i2c_test2);
}
}
if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
{
if (i2c_abuse_test_counter < 16)
{
i2c_abuse_test_counter++;
}
else
{
// wait until ready:
if (i2c_idle(&I2C_ABUSE_PORT))
{
i2c_abuse_test_counter = 1;
i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
i2c_abuse_test_bitrate += 17000;
if (i2c_abuse_test_bitrate > 410000)
{
i2c_abuse_test_bitrate -= 410000;
}
}
}
if (i2c_abuse_test_counter < 16)
{
RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED));
i2c_abuse_send_transaction( i2c_abuse_test_counter );
}
}
}
void main_periodic(void)
{
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_get_motors_on());
SetActuatorsFromCommands(commands, autopilot_get_mode());
if (autopilot_in_flight()) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
void imu_periodic(void)
{
mpu60x0_spi_periodic(&imu_mpu_hmc.mpu);
/* Read HMC58XX every 10 times of main freq
* at ~50Hz (main loop for rotorcraft: 512Hz)
*/
RunOnceEvery(10, hmc58xx_periodic(&imu_mpu_hmc.hmc));
}
void baro_ms5611_periodic_check( void ) {
ms5611_i2c_periodic_check(&baro_ms5611);
#if SENSOR_SYNC_SEND
// send coeff every 30s
RunOnceEvery((30*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff());
#endif
}
