/*
check for driver input on rudder/steering stick for arming/disarming
*/
void Rover::rudder_arm_disarm_check()
{
// In Rover we need to check that its set to the throttle trim and within the DZ
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
if (!channel_throttle->in_trim_dz()) {
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode then disallow rudder arming/disarming
if (control_mode->auto_throttle()) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_steer->get_control_in() > 4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!motor_active() & !g2.motors.have_skid_steering()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
// This is disabled for skid steering otherwise when tring to turn a skid steering rover around
// the rover would disarm
if (channel_steer->get_control_in() < -4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
check for driver input on rudder/steering stick for arming/disarming
*/
void Rover::rudder_arm_disarm_check()
{
// In Rover we need to check that its set to the throttle trim and within the DZ
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
if (!channel_throttle->in_trim_dz()) {
rudder_arm_timer = 0;
return;
}
// check if arming/disarming allowed from this mode
if (!control_mode->allows_arming_from_transmitter()) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_steer->get_control_in() > 4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!g2.motors.active()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
if (channel_steer->get_control_in() < -4000) {
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
task main()
{
while(true)
{
getJoystickSettings(joystick); // Update Buttons and Joysticks
main_wheel( joystick.joy1_x1, joystick.joy1_y1 ); // do not change
central_motor( joystick.joy1_x2 ); // do not change
arm_motors( joystick.joy2_y2 ); //do not change
arm_servos( joystick.joy2_TopHat ); //do not change
claw_servo( joy2Btn(2), joy2Btn(4) ); //do not change
wait1Msec(TIME_INTERVAL);
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (channel_throttle->control_in != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode then disallow rudder arming/disarming
if (auto_throttle_mode ) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->control_in > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->control_in < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (get_throttle_input() != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode and not in CRUISE or FBWB
// modes then disallow rudder arming/disarming
if (auto_throttle_mode &&
(control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
int main(void)
{
//Set data direction, set mux pin to low and turn off LEDs.
DDRD |= _BV(SERIAL_MUX_PIN) | _BV(RED_LED_PIN) | _BV(BLUE_LED_PIN);
PORTD &= ~(_BV(SERIAL_MUX_PIN) | _BV(BLUE_LED_PIN) | _BV(RED_LED_PIN));
//Enable USART and connect to scanf/printf.
USART_init();
USART_to_stdio();
//Initialize sensors and FPGA.
mpu_init();
fpga_init(&PORTB, &DDRB, 2); //Needs to be after mpu_init().
mag_init();
milli_timer_init();
//This is pretty important.
sei();
//Get some initial readings
mpu_scale();
mag_read();
attitude();
mag_calculate(roll, pitch);
//Use those to initialize PID state
pid_init(&pitch_pid, P_CONST, I_CONST, D_CONST, pitch + PITCH_CORRECT);
pid_init(&roll_pid, P_CONST, I_CONST, D_CONST, roll + ROLL_CORRECT);
pid_init(&heading_pid, P_CONST, I_CONST, D_CONST, heading);
//We want to be level and retain heading (for now)
target_pitch = 0;
target_roll = 0;
target_heading = heading;
//Calibrate and arm the motors.
printf("Enter y to calibrate and arm motors.\n");
scanf("%c", &cmd);
if (cmd == 'y')
{
calibrate_motors();
printf("Motors calibrated and armed.\n");
}
else
{
printf("Calibration skipped.\n");
do
{
printf("Enter 'a' to arm motors.\n");
scanf("%c", &cmd);
} while (cmd != 'a');
arm_motors();
PORTD |= _BV(BLUE_LED_PIN);
printf("Motors armed.\n");
}
uint16_t loop_duration;
unsigned long last_loop_time = 0;
uint8_t count = 0;
while(1)
{
//unsigned pt = PID_PERIOD;
//unsigned pf = PID_FREQ;
/*
printf("Period: %u; frequency: %u\n", pt, pf);
//Wait for pid flag to be true.
while (!pid_flag);
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) //Reset it atomically as soon as we start.
{
pid_flag = 0;
}
*/
loop_duration = current_time - last_loop_time;
last_loop_time = current_time;
mpu_scale();
mag_read();
attitude();
//mag_calculate(roll, pitch);
pitch_correct = CORRECTION_GAIN * pid_update(&pitch_pid, target_pitch, pitch + PITCH_CORRECT);
roll_correct = CORRECTION_GAIN * pid_update(&roll_pid, target_roll, roll + ROLL_CORRECT);
//heading_correct = CORRECTION_GAIN * pid_update(&heading_pid, target_heading, heading);
heading_correct = 0;
//Safety: if throttle=0, stop motors.
if (throttle)
{
PORTD &= ~_BV(RED_LED_PIN);
//adjust_attitude(pitch_correct, roll_correct, heading_correct);
pcms[0] = throttle; pcms[1] = throttle; pcms[2] = throttle; pcms[3] = throttle;
set_pcm(pcms);
}
else
{
PORTD |= _BV(RED_LED_PIN);
pcms[0] = 0; pcms[1] = 0; pcms[2] = 0; pcms[3] = 0;
set_pcm(pcms);
//prevent integration windup when stationary
roll_pid.integrated_error = 0;
pitch_pid.integrated_error = 0;
}
++count;
#ifdef PRINTING
if (count % 100 == 0)
{
#ifdef PRINT_CSV
printf("%i,%i,%i,", accel_x, accel_y, accel_z);
printf("%f,%f,%f,", gyro_x, gyro_y, gyro_z);
printf("%i,%i,%i,", mag_x, mag_y, mag_z);
printf("%f,%f,%f,", pitch, roll, heading);
printf("%f,%f,%f,", pitch_correct, roll_correct, heading_correct);
printf("%u,%u,%u,%u,%u", (0x00FF & (unsigned)pcms[0]), (0x00FF & (unsigned)pcms[1]), (0x00FF & (unsigned)pcms[2]), (0x00FF & (unsigned)pcms[3]), (0x00FF & (unsigned)throttle));
printf("%u\n", loop_duration);
#endif
#ifdef PRETTY_PRINT
printf("ax: %i; ay: %i; az: %i; ", accel_x, accel_y, accel_z);
printf("gx: %f; gy: %f; gz: %f; ", gyro_x, gyro_y, gyro_z);
printf("mx: %i; my: %i; mz: %i; ", mag_x, mag_y, mag_z);
printf("p: %f; r: %f; h: %f; ", pitch + PITCH_CORRECT, roll + ROLL_CORRECT, heading_deg);
printf("PC: %f; RC: %f; HC: %f; ", pitch_correct, roll_correct, heading_correct);
printf("fl: %u; fr: %u; rl: %u; rr: %u; ", (0x00FF & (unsigned)pcms[FRONT_LEFT]), (0x00FF & (unsigned)pcms[FRONT_RIGHT]), (0x00FF & (unsigned)pcms[REAR_LEFT]), (0x00FF & (unsigned)pcms[REAR_RIGHT]));
printf("thr: %u; ", (0x00FF & (unsigned)throttle));
printf("t: %u\n", loop_duration);
#endif
}
#endif
//don't send anything other than numbers, dumbass
if (!(count % 64) && (rx_bytes >= 2))
{
if (scanf("%u", &num) != 0x00)
{
if (num > 800)
throttle = 800;
else
throttle = num;
#ifndef PRINTING
printf("Throttle: %u\n", throttle);
#endif
}
}
}
}