You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
60 lines
1.6 KiB
60 lines
1.6 KiB
#include "Blimp.h" |
|
|
|
// return barometric altitude in centimeters |
|
void Blimp::read_barometer(void) |
|
{ |
|
barometer.update(); |
|
|
|
baro_alt = barometer.get_altitude() * 100.0f; |
|
} |
|
|
|
|
|
void Blimp::compass_cal_update() |
|
{ |
|
compass.cal_update(); |
|
|
|
if (hal.util->get_soft_armed()) { |
|
return; |
|
} |
|
|
|
static uint32_t compass_cal_stick_gesture_begin = 0; |
|
|
|
if (compass.is_calibrating()) { |
|
if (channel_yaw->get_control_in() < -4000 && channel_down->get_control_in() > 900) { |
|
compass.cancel_calibration_all(); |
|
} |
|
} else { |
|
bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_down->get_control_in() > 900; |
|
uint32_t tnow = millis(); |
|
|
|
if (!stick_gesture_detected) { |
|
compass_cal_stick_gesture_begin = tnow; |
|
} else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) { |
|
#ifdef CAL_ALWAYS_REBOOT |
|
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true); |
|
#else |
|
compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false); |
|
#endif |
|
} |
|
} |
|
} |
|
|
|
void Blimp::accel_cal_update() |
|
{ |
|
if (hal.util->get_soft_armed()) { |
|
return; |
|
} |
|
ins.acal_update(); |
|
// check if new trim values, and set them |
|
float trim_roll, trim_pitch; |
|
if (ins.get_new_trim(trim_roll, trim_pitch)) { |
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); |
|
} |
|
|
|
#ifdef CAL_ALWAYS_REBOOT |
|
if (ins.accel_cal_requires_reboot()) { |
|
hal.scheduler->delay(1000); |
|
hal.scheduler->reboot(false); |
|
} |
|
#endif |
|
}
|
|
|