int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_control_in(),
g.rc_2.get_control_in(),
channel_throttle->get_control_in(),
g.rc_4.get_control_in(),
g.rc_5.get_control_in(),
g.rc_6.get_control_in(),
g.rc_7.get_control_in(),
g.rc_8.get_control_in());
if(cliSerial->available() > 0){
return (0);
}
}
}
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1) {
hal.scheduler->delay(20);
read_radio();
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_throttle->calc_pwm();
channel_rudder->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
(int)channel_roll->control_in,
(int)channel_pitch->control_in,
(int)channel_throttle->control_in,
(int)channel_rudder->control_in,
(int)g.rc_5.control_in,
(int)g.rc_6.control_in,
(int)g.rc_7.control_in,
(int)g.rc_8.control_in);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int main(void)
{
int i;
clock_init();
gpio_init();
servo_init();
//gpio_set(GPIOC, GPIO13);
for(i=0;i<100;i++){
gpio_toggle(GPIOC, GPIO13); /* LED on/off */
delay(1500000);
}
gpio_clear(GPIOC, GPIO13);
// let pan-til "look around a little"
while(1) {
set_servos(SERVO_MIN, SERVO_MAX);
set_servos(SERVO_NULL, SERVO_NULL);
set_servos(SERVO_MAX, SERVO_MIN);
set_servos(SERVO_NULL, SERVO_NULL);
set_servos(SERVO_MIN, SERVO_MIN);
set_servos(SERVO_MAX, SERVO_MAX);
set_servos(SERVO_NULL, SERVO_NULL);
set_servos(SERVO_MIN, SERVO_NULL);
set_servos(SERVO_MAX, SERVO_NULL);
set_servos(SERVO_NULL, SERVO_NULL);
set_servos(SERVO_NULL, SERVO_MIN);
set_servos(SERVO_NULL, SERVO_MAX);
set_servos(SERVO_NULL, SERVO_NULL);
}
return 0;
}
// This routine is called repeatedly - its your main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){
TICK_COUNT frameEnd = frameStart + 20000;
wdt_reset();
// very occasionally RF module misses interrupt.
// this usually happens when signal is weak.
// testing interrupt pin manually here is an effective workaround.
//if (pin_is_low(E7)){
// cyrfInterupt(&cyrf_0);
//}
//cyrf6936_RX_mode(&cyrf_0);
// poll sensors as often as possible
// get readings from A2D.
//int tmp = a2dConvert8bit(ADC_CH_ADC6);
// = ;
currDraw += a2dConvert10bit(ADC_CH_ADC1);
rcAccelX += a2dConvert10bit(ADC_CH_ADC7);
rcAccelY += a2dConvert10bit(ADC_CH_ADC6);
rcAccelZ += a2dConvert10bit(ADC_CH_ADC5);
rcGyroX += a2dConvert10bit(ADC_CH_ADC4);
rcGyroY += a2dConvert10bit(ADC_CH_ADC3);
sensorCount++; // keep track of number of times ADC has been read.
int tmp;
if(frameEnd <= clockGetus()) {
frameStart = frameEnd;
// this section happens every 20ms frame.
// frame counter.
data[RX_FRA_NUM]++;
data[RX_BAT_CUR] = (currDraw / sensorCount) >>2;
// average accelerometer.
RAcc1.RXACC = (rcAccelX / sensorCount) -AccOffset;
RAcc1.RYACC = (rcAccelY / sensorCount) -AccOffset;
RAcc1.RZACC = (rcAccelZ / sensorCount) -AccOffset;
RAccSize1 = sqrt((RAcc1.RXACC*RAcc1.RXACC)+(RAcc1.RYACC*RAcc1.RYACC)+(RAcc1.RZACC*RAcc1.RZACC));
// extract roll and pitch from accelerometer's 3 axis.
data[RX_ACCEL_Y] = AtAcc.AXZ = 90*atan2(RAcc1.RXACC,RAcc1.RZACC) + data[RX_AUTOP_Y_TRIM] - 0x7F;
data[RX_ACCEL_Y] += 0x7F;
data[RX_ACCEL_X] = AtAcc.AYZ = 90*atan2(RAcc1.RYACC,RAcc1.RZACC) + data[RX_AUTOP_X_TRIM] - 0x7F;
data[RX_ACCEL_X] += 0x7F;
// *** experimental ***
// multiply accelerometer readings by a factor of current draw.
data[RX_ACCEL_Y] = AtAcc.AXZ = AtAcc.AXZ - data[RX_BAT_CUR]/4.5;
data[RX_ACCEL_Y] += 0x7F;
data[RX_ACCEL_X] = AtAcc.AYZ = AtAcc.AYZ - data[RX_BAT_CUR]/5.5;
data[RX_ACCEL_X] += 0x7F;
// average gyro.
int x_gyro_offset = data[RX_GYRO_X_OFFSET_L] + (0x100*data[RX_GYRO_X_OFFSET_L]);
int y_gyro_offset = data[RX_GYRO_Y_OFFSET_L] + (0x100*data[RX_GYRO_Y_OFFSET_L]);
data[RX_GYRO_X] = AtGyr.AYZ = (rcGyroX / sensorCount);
data[RX_GYRO_X] += 0x7F - x_gyro_offset;
data[RX_GYRO_Y] = AtGyr.AXZ = (rcGyroY / sensorCount);
data[RX_GYRO_Y] += 0x7F - y_gyro_offset;
// work out estimated angle based on previous estimate and sensor data.
data[RX_EST_X] = AtEst1.AYZ = (((AtEst0.AYZ + ((AtGyr.AYZ - x_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AYZ)) / (1 + GyroBias);
data[RX_EST_X] += 0x7F;
data[RX_EST_Y] = AtEst1.AXZ = (((AtEst0.AXZ + ((AtGyr.AXZ - y_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AXZ)) / (1 + GyroBias);
data[RX_EST_Y] += 0x7F;
// something mental is going on at power on so stop excessive values.
if(AtEst1.AYZ > (0x2A7FF + 90)){ AtEst1.AYZ = (0x2A7FF + 90);}
if(AtEst1.AYZ < 0x2A7FF - 90){ AtEst1.AYZ = (0x2A7FF - 90);}
if(AtEst1.AXZ > (0x38DFF + 90)){ AtEst1.AXZ = (0x38DFF + 90);}
if(AtEst1.AXZ < 0x38DFF - 90){ AtEst1.AXZ = (0x38DFF - 90);}
//data[RX_EST_X] = data[RX_ACCEL_X];
//data[RX_EST_Y] = data[RX_ACCEL_Y];
//rprintfu16((int)(AtGyr.AXZ/0x10000) + 0x8000);
//rprintfu16(AtGyr.AXZ );
//rprintfProgStrM("\n");
currDraw = rcAccelX = rcAccelY = rcAccelZ = rcGyroX = rcGyroY = 0;
AtEst0 = AtEst1;
sensorCount1=sensorCount;
sensorCount=0;
/* // read battery and current sensor
tmp = a2dConvert8bit(ADC_CH_ADC0);
if(tmp > data[RX_BAT_VOLT]){
data[RX_BAT_VOLT]++;
}
else if(tmp < data[RX_BAT_VOLT]){
data[RX_BAT_VOLT]--;
}
data[RX_BAT_CUR] = a2dConvert8bit(ADC_CH_ADC1);
*/
if(!(testIfPacketReceived(data)==0)){
// this section done if a packet has been received this frame.
// (the data[] should have arrived during an interrupt on the RF module IRQ pin.)
uptime++;
// set servo positions.
set_servos(data);
pin_high(C1); // warning LED off
// watch for PS2 controller button presses.
// these are de-bounced by running a simple counter.
uint16_t tmp = ((data[TX_PS2_0] <<8) + data[TX_PS2_1]);
uint8_t i;
for (i=0; i<16; i++){
if(tmp & (1<<i)){
// button pressed. increase counter
//rprintfProgStrM("\nPressed:"); rprintfu08(i);
if (buttonPressed[i] < 0xFF){ buttonPressed[i]++; }
}
else if(buttonPressed[i] > 0){
// button has just been released. save value in counter to de-bounce buttons.
//rprintfProgStrM("\nReleased:"); rprintfu08(i);
buttonReleased[i]=buttonPressed[i];
buttonPressed[i]=0;
}
else{
buttonReleased[i]=0;
buttonPressed[i]=0;
}
}
if (buttonReleased[PS2_B_BTN_TRIANGLE] > 5){
set_trims(data);
}
if (buttonReleased[PS2_B_BTN_CIRCLE] > 5){
reset_trims();
}
if (buttonReleased[PS2_B_BTN_X] > 5){
data[RX_GYRO_X_OFFSET_H] = (uint8_t)(AtGyr.AYZ / 0x100);
data[RX_GYRO_X_OFFSET_L] = (uint8_t)(AtGyr.AYZ - (AtGyr.AYZ*0x100));
data[RX_GYRO_Y_OFFSET_H] = (uint8_t)(AtGyr.AXZ / 0x100);
data[RX_GYRO_Y_OFFSET_L] = (uint8_t)(AtGyr.AXZ - (AtGyr.AXZ*0x100));
eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_H], data[RX_GYRO_X_OFFSET_H]);
eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_L], data[RX_GYRO_X_OFFSET_L]);
eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H], data[RX_GYRO_Y_OFFSET_H]);
eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L], data[RX_GYRO_Y_OFFSET_L]);
}
if (buttonReleased[PS2_B_DPAD_UP] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
//increase autopilot sensitivity (Y axis).
data[RX_AUTOP_Y_MUL] ++;
}
else{
//increase autopilot trim (Y axis).
data[RX_AUTOP_Y_TRIM] ++;
}
}
else if (data[RX_SERVO_2_MUL] < 137){
data[RX_SERVO_2_MUL]++;
}
}
if (buttonReleased[PS2_B_DPAD_DOWN] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
if (data[RX_AUTOP_Y_MUL] > 2){
//decrease aoutopilot sensitivity (Y axis).
data[RX_AUTOP_Y_MUL] --;
}
}
else{
//decrease autopilot trim (Y axis).
data[RX_AUTOP_Y_TRIM] --;
}
}
else if (data[RX_SERVO_2_MUL] > 117){
data[RX_SERVO_2_MUL]--;
}
}
if (buttonReleased[PS2_B_DPAD_RIGHT] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
//increase aoutopilot sensitivity (X axis).
data[RX_AUTOP_X_MUL] ++;
}
else{
//increase autopilot trim (X axis).
data[RX_AUTOP_X_TRIM] ++;
}
}
else if (data[RX_SERVO_4_MUL] < 137){
data[RX_SERVO_4_MUL]++;
data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL];
}
}
if (buttonReleased[PS2_B_DPAD_LEFT] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
if (data[RX_AUTOP_X_MUL] > 2){
//decrease autopilot sensitivity (X axis).
data[RX_AUTOP_X_MUL] --;
}
}
else{
//decrease autopilot trim (X axis).
data[RX_AUTOP_X_TRIM] --;
}
}
else if (data[RX_SERVO_4_MUL] > 117){
data[RX_SERVO_4_MUL]--;
data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL];
}
}
}
else{
// no packet received this frame
pin_low(C1); // warning LED on
}
// enable receive mode.
cyrf6936_RX_mode(&cyrf_0);
cyrf6936_RX_mode(&cyrf_1);
if(data[RX_PAC_GAP] > 25){
// no data received for 1/2 second
// so clear PS2 controller readings
// and put servos to default positions.
if(data[TX_CONTROLER]==0){
data[TX_PS2_LY] = 128;
}
else{
data[TX_PS2_LY] = 254;
}
data[TX_PS2_LX] = 128;
data[TX_PS2_RY] = 128;
data[TX_PS2_RX] = 128;
set_servos(data);
//pin_low(C0); // warning LED on
}
else{
//pin_high(C0); // warning LED off
}
// debug status LED off
pin_high(C0);
}
