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 }