static int rf_encoder_changed(void *data) {
int pos=*(int*)data;
if(pos!=0) {
if(function || tune) {
// tune drive
double d=getTuneDrive();
d+=(double)pos;
if(d<0.0) {
d=0.0;
} else if(d>100.0) {
d=100.0;
}
set_tune(d);
} else {
// drive
double d=getDrive();
d+=(double)pos;
if(d<0.0) {
d=0.0;
} else if(d>100.0) {
d=100.0;
}
set_drive(d);
}
}
free(data);
return 0;
}
/**
* Blink red LED and play negative tune (if use_buzzer == true).
*/
void tune_negative(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
}
/**
* Blink white LED and play neutral tune (if use_buzzer == true).
*/
void tune_neutral(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
void tune_home_set(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_HOME_SET);
}
}
int main(void)
{
uint8_t mode;
// set which tunes are played for foil or epee modes
set_tune();
// infinite loop to wait for PCI interrupt and process hit
while(1) {
cli();
mode = getmode();
// got mode --- foil or !foil
DDRB = 0; // set all pins to input to save power
PORTB = 0xFF; // set pullups
set_sleep_mode(SLEEP_MODE_PWR_DOWN); // set sleep mode
// set up PCI interrupts
PCMSK |= _BV(INPIN); // enable PCI interrupt on INPIN
GIFR = _BV(PCIF); // clear any pending PCI interrupts
GIMSK |= _BV(PCIE); // enable pin change interrupt
sei(); // enable interrupts
// go to sleep until there is an interrupt
sleep_mode();
if (mode) {
PLAYTIME = epee_song;
epee_handle();
}
else {
PLAYTIME = foil_song;
foil_handle();
}
} // end infinite while loop
}
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void *worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[80];
pendingStr[0] = 0;
for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
}
}
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(20000);
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
usleep(20000);
continue;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok) {
break;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
// Note that this side is complete
side_data_collected[orient] = true;
// output neutral tune
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
// temporary priority boost for the white blinking led to come trough
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
