mirror of https://github.com/ArduPilot/ardupilot
Fixed GPS activation issue, Serial/Serial3 selection and new automatic mode switch feature
git-svn-id: https://arducopter.googlecode.com/svn/trunk@445 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
dff31e7dde
commit
837c183922
|
|
@ -212,6 +212,8 @@ int Sonar_Counter=0;
|
|||
|
||||
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
|
||||
byte AP_mode = 2;
|
||||
byte FL_mode = 0; // Flight mode : 0 => Stable, 1 => Acro mode. Stable as default
|
||||
|
||||
|
||||
// Mode LED timers and variables, used to blink LED_Green
|
||||
byte gled_status = HIGH;
|
||||
|
|
@ -248,4 +250,4 @@ long tlmTimer = 0;
|
|||
|
||||
// Arming/Disarming
|
||||
uint8_t Arming_counter=0;
|
||||
uint8_t Disarming_counter=0;
|
||||
uint8_t Disarming_counter=0;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
1) A, B, C LED's blinking rapidly while waiting ESCs to bootup and initial shake to end from connecting battery
|
||||
2) A, B, C LED's have running light while calibrating Gyro/Acc's
|
||||
3) Green LED Solid after initialization finished
|
||||
|
||||
|
||||
Green LED On = APM Initialization Finished
|
||||
Yellow LED On = GPS Hold Mode
|
||||
Yellow LED Off = Flight Assist Mode (No GPS)
|
||||
|
|
@ -49,11 +49,13 @@
|
|||
/* User definable modules */
|
||||
|
||||
// Comment out with // modules that you are not using
|
||||
#define IsGPS // Do we have a GPS connected
|
||||
#define IsGPS // Do we have a GPS connected
|
||||
//#define IsNEWMTEK// Do we have MTEK with new firmware
|
||||
#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
|
||||
#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
|
||||
//#define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port
|
||||
#define IsAM // Do we have motormount LED's. AM = Atraction Mode
|
||||
#define IsAM // Do we have motormount LED's. AM = Atraction Mode
|
||||
#define AUTOMODE // New experimental Automode to change between Stable <=> Acro. If pitch/roll stick move is more than 50% change mode
|
||||
//#define IsXBEE // Moves all serial communication to Telemetry port when activated.
|
||||
|
||||
#define CONFIGURATOR // Do se use Configurator or normal text output over serial link
|
||||
|
||||
|
|
@ -73,6 +75,26 @@
|
|||
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
|
||||
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
|
||||
|
||||
// Quick and easy hack to change FTDI Serial output to Telemetry port. Just activate #define IsXBEE some lines earlier
|
||||
#ifndef IsXBEE
|
||||
#define SerBau 115200
|
||||
#define SerPri Serial.print
|
||||
#define SerPriln Serial.println
|
||||
#define SerAva Serial.available
|
||||
#define SerRea Serial.read
|
||||
#define SerFlu Serial.flush
|
||||
#define SerBeg Serial.begin
|
||||
#define SerPor "FTDI"
|
||||
#else
|
||||
#define SerBau 115200
|
||||
#define SerPri Serial3.print
|
||||
#define SerPriln Serial3.println
|
||||
#define SerAva Serial3.available
|
||||
#define SerRea Serial3.read
|
||||
#define SerFlu Serial3.flush
|
||||
#define SerBeg Serial3.begin
|
||||
#define SerPor "Telemetry"
|
||||
#endif
|
||||
|
||||
/* ****************************************************************************** */
|
||||
/* ****************************** Includes ************************************** */
|
||||
|
|
@ -349,16 +371,19 @@ void setup()
|
|||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
||||
//Serial.begin(57600);
|
||||
Serial.begin(115200);
|
||||
//Serial.println();
|
||||
//Serial.println("ArduCopter Quadcopter v1.0");
|
||||
SerBeg(SerBau); // Initialize SerialXX.port, IsXBEE define declares which port
|
||||
#ifndef CONFIGURATOR
|
||||
SerPri("ArduCopter Quadcopter v");
|
||||
SerPriln(VER)
|
||||
SerPri("Serial ready on port: "); // Printout greeting to selecter serial port
|
||||
SerPriln(SerPor); // Printout serial port name
|
||||
#endif
|
||||
|
||||
// Check if we enable the DataFlash log Read Mode (switch)
|
||||
// If we press switch 1 at startup we read the Dataflash eeprom
|
||||
while (digitalRead(SW1_pin)==0)
|
||||
{
|
||||
Serial.println("Entering Log Read Mode...");
|
||||
SerPriln("Entering Log Read Mode...");
|
||||
Log_Read(1,2000);
|
||||
delay(30000);
|
||||
}
|
||||
|
|
@ -382,10 +407,10 @@ void setup()
|
|||
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
|
||||
{
|
||||
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
|
||||
//Serial.print(AN[y]);
|
||||
//Serial.print(",");
|
||||
//SerPri(AN[y]);
|
||||
//SerPri(",");
|
||||
}
|
||||
//Serial.println();
|
||||
//SerPriln();
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
delay(10);
|
||||
|
||||
|
|
@ -419,12 +444,12 @@ void setup()
|
|||
#ifndef CONFIGURATOR
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
SerPri("AN[]:");
|
||||
SerPriln(AN_OFFSET[i]);
|
||||
}
|
||||
Serial.print("Yaw neutral value:");
|
||||
// Serial.println(Neutro_yaw);
|
||||
Serial.print(yaw_mid);
|
||||
SerPri("Yaw neutral value:");
|
||||
// SerPriln(Neutro_yaw);
|
||||
SerPri(yaw_mid);
|
||||
#endif
|
||||
delay(1000);
|
||||
|
||||
|
|
@ -492,16 +517,16 @@ void loop(){
|
|||
log_yaw = ToDeg(yaw) * 10;
|
||||
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
SerPri(log_roll);
|
||||
SerPri(",");
|
||||
SerPri(log_pitch);
|
||||
SerPri(",");
|
||||
SerPri(log_yaw);
|
||||
|
||||
//for (int i = 0; i < 6; i++)
|
||||
//{
|
||||
// Serial.print(AN[i]);
|
||||
// Serial.print(",");
|
||||
// SerPri(AN[i]);
|
||||
// SerPri(",");
|
||||
//}
|
||||
#endif
|
||||
|
||||
|
|
@ -524,6 +549,28 @@ void loop(){
|
|||
|
||||
command_rx_roll = (ch_roll-roll_mid) / 12.0;
|
||||
command_rx_pitch = (ch_pitch-pitch_mid) / 12.0;
|
||||
|
||||
#ifdef AUTOMODE
|
||||
// New Automatic Stable <=> Acro switch. If pitch/roll stick is more than 60% from center, change to Acro
|
||||
if(command_rx_roll >= 30 || command_rx_roll <= -30 ||
|
||||
command_rx_pitch >= 30 || command_rx_pitch <= -30 ) {
|
||||
FL_mode = 1;
|
||||
} else FL_mode = 0;
|
||||
#endif
|
||||
|
||||
if(ch_aux2 > 1800) FL_mode = 1; // Force to Acro mode from radio
|
||||
|
||||
/*
|
||||
// Debuging channels and fl_mode
|
||||
SerPri(command_rx_roll);
|
||||
comma();
|
||||
SerPri(command_rx_pitch);
|
||||
comma();
|
||||
SerPri(FL_mode, DEC);
|
||||
SerPriln();
|
||||
*/
|
||||
|
||||
|
||||
//aux_float = (ch_yaw-Neutro_yaw) / 180.0;
|
||||
if (abs(ch_yaw-yaw_mid)<12) // Take into account a bit of "dead zone" on yaw
|
||||
aux_float = 0.0;
|
||||
|
|
@ -540,11 +587,16 @@ void loop(){
|
|||
//K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
|
||||
K_aux = K_aux * 0.8 + ((ch_aux2-AUX_MID) / 300.0 + 1.7) * 0.2; // /300 + 1.0
|
||||
if (K_aux < 0) K_aux = 0;
|
||||
//Serial.print(",");
|
||||
//Serial.print(K_aux);
|
||||
//SerPri(",");
|
||||
//SerPri(K_aux);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// We read the Quad Mode from Channel 5
|
||||
if (ch_aux < 1200)
|
||||
if (ch_aux > 1800) // We really need to switch it ON from radio to activate GPS hold
|
||||
{
|
||||
AP_mode = 1; // Position hold mode (GPS position control)
|
||||
digitalWrite(LED_Yellow,HIGH); // Yellow LED On
|
||||
|
|
@ -567,11 +619,11 @@ void loop(){
|
|||
target_longitude = GPS.Longitude;
|
||||
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.println();
|
||||
Serial.print("* Target:");
|
||||
Serial.print(target_longitude);
|
||||
Serial.print(",");
|
||||
Serial.println(target_lattitude);
|
||||
SerPriln();
|
||||
SerPri("* Target:");
|
||||
SerPri(target_longitude);
|
||||
SerPri(",");
|
||||
SerPriln(target_lattitude);
|
||||
#endif
|
||||
target_position=1;
|
||||
//target_sonar_altitude = sonar_value;
|
||||
|
|
@ -595,10 +647,10 @@ void loop(){
|
|||
GPS.NewData=0; // We Reset the flag...
|
||||
|
||||
//Output GPS data
|
||||
//Serial.print(",");
|
||||
//Serial.print(GPS.Lattitude);
|
||||
//Serial.print(",");
|
||||
//Serial.print(GPS.Longitude);
|
||||
//SerPri(",");
|
||||
//SerPri(GPS.Lattitude);
|
||||
//SerPri(",");
|
||||
//SerPri(GPS.Longitude);
|
||||
|
||||
// Write GPS data to DataFlash log
|
||||
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
|
||||
|
|
@ -617,15 +669,20 @@ void loop(){
|
|||
}
|
||||
else
|
||||
{
|
||||
//Serial.print("NOFIX");
|
||||
//SerPri("NOFIX");
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Control methodology selected using AUX2
|
||||
if (ch_aux2 < 1200) {
|
||||
// if (ch_aux2 < 1200) {
|
||||
if(FL_mode == 0) { // Changed for variable
|
||||
gled_speed = 1200;
|
||||
Attitude_control_v3();
|
||||
}
|
||||
|
|
@ -714,7 +771,7 @@ void loop(){
|
|||
APM_RC.Force_Out2_Out3();
|
||||
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.println(); // Line END
|
||||
SerPriln(); // Line END
|
||||
#endif
|
||||
}
|
||||
#ifdef CONFIGURATOR
|
||||
|
|
@ -749,4 +806,4 @@ void loop(){
|
|||
// END of Arducopter.pde
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -27,23 +27,23 @@ void RadioCalibration() {
|
|||
long pitch_new = 0;
|
||||
long yaw_new = 0;
|
||||
|
||||
Serial.flush();
|
||||
Serial.println("Entering Radio Calibration mode");
|
||||
Serial.println("Current channel MID values are:");
|
||||
Serial.print("ROLL: ");
|
||||
Serial.print(roll_mid);
|
||||
Serial.print(" PITCH: ");
|
||||
Serial.print(pitch_mid);
|
||||
Serial.print(" YAW: ");
|
||||
Serial.print(yaw_mid);
|
||||
Serial.println();
|
||||
Serial.println("Recalibrate Channel MID's [Y/N]?: ");
|
||||
SerFlu();
|
||||
SerPriln("Entering Radio Calibration mode");
|
||||
SerPriln("Current channel MID values are:");
|
||||
SerPri("ROLL: ");
|
||||
SerPri(roll_mid);
|
||||
SerPri(" PITCH: ");
|
||||
SerPri(pitch_mid);
|
||||
SerPri(" YAW: ");
|
||||
SerPri(yaw_mid);
|
||||
SerPriln();
|
||||
SerPriln("Recalibrate Channel MID's [Y/N]?: ");
|
||||
command_timer = millis();
|
||||
|
||||
// Start counter loop and wait serial input. If not input for 5 seconds, return to normal mode
|
||||
while(millis() - command_timer < 5000) {
|
||||
if (Serial.available()) {
|
||||
queryType = Serial.read();
|
||||
if (SerAva()) {
|
||||
queryType = SerRea();
|
||||
if(queryType == 'y' || queryType == 'Y') {
|
||||
Cmd_ok = TRUE;
|
||||
break;
|
||||
|
|
@ -56,16 +56,16 @@ void RadioCalibration() {
|
|||
}
|
||||
if(Cmd_ok) {
|
||||
// We have a go. Let's do new calibration
|
||||
Serial.println("Starting calibration run in 5 seconds. Place all sticks to their middle including trims");
|
||||
SerPriln("Starting calibration run in 5 seconds. Place all sticks to their middle including trims");
|
||||
for(counter = 5; counter >= 0; counter --) {
|
||||
command_timer = millis();
|
||||
while(millis() - command_timer < 1000) {
|
||||
}
|
||||
Serial.println(counter);
|
||||
SerPriln(counter);
|
||||
}
|
||||
// Do actual calibration now
|
||||
Serial.println("Measuring average channel values");
|
||||
Serial.println("ROLL, PITCH, YAW");
|
||||
SerPriln("Measuring average channel values");
|
||||
SerPriln("ROLL, PITCH, YAW");
|
||||
|
||||
counter = 0; // Reset counter for just in case.
|
||||
command_timer = millis();
|
||||
|
|
@ -80,42 +80,42 @@ void RadioCalibration() {
|
|||
ch_aux = APM_RC.InputCh(4);
|
||||
ch_aux2 = APM_RC.InputCh(5);
|
||||
|
||||
Serial.print(ch_roll);
|
||||
SerPri(ch_roll);
|
||||
comma();
|
||||
Serial.print(ch_pitch);
|
||||
SerPri(ch_pitch);
|
||||
comma();
|
||||
Serial.print(ch_yaw);
|
||||
Serial.println();
|
||||
SerPri(ch_yaw);
|
||||
SerPriln();
|
||||
roll_new += ch_roll;
|
||||
pitch_new += ch_pitch;
|
||||
yaw_new += ch_yaw;
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
Serial.print("New samples received: ");
|
||||
Serial.println(counter);
|
||||
SerPri("New samples received: ");
|
||||
SerPriln(counter);
|
||||
roll_new = roll_new / counter;
|
||||
pitch_new = pitch_new / counter;
|
||||
yaw_new = yaw_new / counter;
|
||||
Serial.print("New values as: ");
|
||||
Serial.print("ROLL: ");
|
||||
Serial.print(roll_new);
|
||||
Serial.print(" PITCH: ");
|
||||
Serial.print(pitch_new);
|
||||
Serial.print(" YAW: ");
|
||||
Serial.print(yaw_new);
|
||||
Serial.println();
|
||||
Serial.println("Accept & Save values [Y/N]?: ");
|
||||
SerPri("New values as: ");
|
||||
SerPri("ROLL: ");
|
||||
SerPri(roll_new);
|
||||
SerPri(" PITCH: ");
|
||||
SerPri(pitch_new);
|
||||
SerPri(" YAW: ");
|
||||
SerPri(yaw_new);
|
||||
SerPriln();
|
||||
SerPriln("Accept & Save values [Y/N]?: ");
|
||||
Cmd_ok = FALSE;
|
||||
while(millis() - command_timer < 5000) {
|
||||
if (Serial.available()) {
|
||||
queryType = Serial.read();
|
||||
if (SerAva()) {
|
||||
queryType = SerRea();
|
||||
if(queryType == 'y' || queryType == 'Y') {
|
||||
Cmd_ok = TRUE;
|
||||
roll_mid = roll_new;
|
||||
pitch_mid = pitch_new;
|
||||
yaw_mid = yaw_new;
|
||||
Serial.println("Values accepted, remember to save them to EEPROM with 'W' command");
|
||||
SerPriln("Values accepted, remember to save them to EEPROM with 'W' command");
|
||||
break;
|
||||
}
|
||||
else {
|
||||
|
|
@ -127,12 +127,12 @@ void RadioCalibration() {
|
|||
}
|
||||
if(queryType == 'n' || queryType == 'N') Cmd_ok = TRUE;
|
||||
if(Cmd_ok)
|
||||
Serial.println("Returning normal mode...");
|
||||
else Serial.println("Command timeout, returning normal mode....");
|
||||
SerPriln("Returning normal mode...");
|
||||
else SerPriln("Command timeout, returning normal mode....");
|
||||
}
|
||||
|
||||
void comma() {
|
||||
Serial.print(',');
|
||||
SerPri(',');
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == 1
|
||||
|
|
|
|||
|
|
@ -91,21 +91,21 @@ void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
|
|||
// Read a Sensor raw data packet
|
||||
void Log_Read_Sensor()
|
||||
{
|
||||
Serial.print("SENSOR:");
|
||||
Serial.print(DataFlash.ReadInt()); // GX
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // GY
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // GZ
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCX
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCY
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCZ
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // AUX
|
||||
Serial.println();
|
||||
SerPri("SENSOR:");
|
||||
SerPri(DataFlash.ReadInt()); // GX
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // GY
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // GZ
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // ACCX
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // ACCY
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // ACCZ
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // AUX
|
||||
SerPriln();
|
||||
}
|
||||
|
||||
// Read an attitude packet
|
||||
|
|
@ -118,29 +118,29 @@ void Log_Read_Attitude()
|
|||
log_roll = DataFlash.ReadInt();
|
||||
log_pitch = DataFlash.ReadInt();
|
||||
log_yaw = DataFlash.ReadInt();
|
||||
Serial.print("ATT:");
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
Serial.println();
|
||||
SerPri("ATT:");
|
||||
SerPri(log_roll);
|
||||
SerPri(",");
|
||||
SerPri(log_pitch);
|
||||
SerPri(",");
|
||||
SerPri(log_yaw);
|
||||
SerPriln();
|
||||
}
|
||||
|
||||
// Read a Sensor raw data packet
|
||||
void Log_Read_PID()
|
||||
{
|
||||
Serial.print("PID:");
|
||||
Serial.print((int)DataFlash.ReadByte()); // NUM_PID
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // P
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // I
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // D
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // output
|
||||
Serial.println();
|
||||
SerPri("PID:");
|
||||
SerPri((int)DataFlash.ReadByte()); // NUM_PID
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // P
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // I
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // D
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt()); // output
|
||||
SerPriln();
|
||||
}
|
||||
|
||||
// Read a GPS packet
|
||||
|
|
@ -164,42 +164,42 @@ void Log_Read_GPS()
|
|||
log_Ground_Speed = DataFlash.ReadLong();
|
||||
log_Ground_Course = DataFlash.ReadLong();
|
||||
|
||||
Serial.print("GPS:");
|
||||
Serial.print(log_Time);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_Fix);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_NumSats);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Lattitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Longitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Altitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Speed);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Course);
|
||||
Serial.println();
|
||||
SerPri("GPS:");
|
||||
SerPri(log_Time);
|
||||
SerPri(",");
|
||||
SerPri((int)log_Fix);
|
||||
SerPri(",");
|
||||
SerPri((int)log_NumSats);
|
||||
SerPri(",");
|
||||
SerPri(log_Lattitude);
|
||||
SerPri(",");
|
||||
SerPri(log_Longitude);
|
||||
SerPri(",");
|
||||
SerPri(log_Altitude);
|
||||
SerPri(",");
|
||||
SerPri(log_Ground_Speed);
|
||||
SerPri(",");
|
||||
SerPri(log_Ground_Course);
|
||||
SerPriln();
|
||||
|
||||
}
|
||||
|
||||
// Read an Radio packet
|
||||
void Log_Read_Radio()
|
||||
{
|
||||
Serial.print("RADIO:");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.println();
|
||||
SerPri("RADIO:");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPriln();
|
||||
}
|
||||
|
||||
// Read the DataFlash log memory : Packet Parser
|
||||
|
|
@ -247,8 +247,8 @@ void Log_Read(int start_page, int end_page)
|
|||
log_step++;
|
||||
break;
|
||||
default:
|
||||
Serial.print("Error Reading Packet: ");
|
||||
Serial.print(packet_count);
|
||||
SerPri("Error Reading Packet: ");
|
||||
SerPri(packet_count);
|
||||
log_step=0; // Restart, we have a problem...
|
||||
}
|
||||
break;
|
||||
|
|
@ -256,12 +256,12 @@ void Log_Read(int start_page, int end_page)
|
|||
if(data==END_BYTE)
|
||||
packet_count++;
|
||||
else
|
||||
Serial.println("Error Reading END_BYTE");
|
||||
SerPriln("Error Reading END_BYTE");
|
||||
log_step=0; // Restart sequence: new packet...
|
||||
}
|
||||
}
|
||||
Serial.print("Number of packets read: ");
|
||||
Serial.println(packet_count);
|
||||
SerPri("Number of packets read: ");
|
||||
SerPriln(packet_count);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -20,8 +20,8 @@
|
|||
|
||||
void readSerialCommand() {
|
||||
// Check for serial message
|
||||
if (Serial.available()) {
|
||||
queryType = Serial.read();
|
||||
if (SerAva()) {
|
||||
queryType = SerRea();
|
||||
switch (queryType) {
|
||||
case 'A': // Stable PID
|
||||
KP_QUAD_ROLL = readFloatSerial();
|
||||
|
|
@ -114,114 +114,114 @@ void sendSerialTelemetry() {
|
|||
float aux_float[3]; // used for sensor calibration
|
||||
switch (queryType) {
|
||||
case '=': // Reserved debug command to view any variable from Serial Monitor
|
||||
/* Serial.print("throttle =");
|
||||
Serial.println(ch_throttle);
|
||||
Serial.print("control roll =");
|
||||
Serial.println(control_roll-CHANN_CENTER);
|
||||
Serial.print("control pitch =");
|
||||
Serial.println(control_pitch-CHANN_CENTER);
|
||||
Serial.print("control yaw =");
|
||||
Serial.println(control_yaw-CHANN_CENTER);
|
||||
Serial.print("front left yaw =");
|
||||
Serial.println(frontMotor);
|
||||
Serial.print("front right yaw =");
|
||||
Serial.println(rightMotor);
|
||||
Serial.print("rear left yaw =");
|
||||
Serial.println(leftMotor);
|
||||
Serial.print("rear right motor =");
|
||||
Serial.println(backMotor);
|
||||
Serial.println();
|
||||
/* SerPri("throttle =");
|
||||
SerPriln(ch_throttle);
|
||||
SerPri("control roll =");
|
||||
SerPriln(control_roll-CHANN_CENTER);
|
||||
SerPri("control pitch =");
|
||||
SerPriln(control_pitch-CHANN_CENTER);
|
||||
SerPri("control yaw =");
|
||||
SerPriln(control_yaw-CHANN_CENTER);
|
||||
SerPri("front left yaw =");
|
||||
SerPriln(frontMotor);
|
||||
SerPri("front right yaw =");
|
||||
SerPriln(rightMotor);
|
||||
SerPri("rear left yaw =");
|
||||
SerPriln(leftMotor);
|
||||
SerPri("rear right motor =");
|
||||
SerPriln(backMotor);
|
||||
SerPriln();
|
||||
|
||||
Serial.print("current roll rate =");
|
||||
Serial.println(read_adc(0));
|
||||
Serial.print("current pitch rate =");
|
||||
Serial.println(read_adc(1));
|
||||
Serial.print("current yaw rate =");
|
||||
Serial.println(read_adc(2));
|
||||
Serial.print("command rx yaw =");
|
||||
Serial.println(command_rx_yaw);
|
||||
Serial.println();
|
||||
SerPri("current roll rate =");
|
||||
SerPriln(read_adc(0));
|
||||
SerPri("current pitch rate =");
|
||||
SerPriln(read_adc(1));
|
||||
SerPri("current yaw rate =");
|
||||
SerPriln(read_adc(2));
|
||||
SerPri("command rx yaw =");
|
||||
SerPriln(command_rx_yaw);
|
||||
SerPriln();
|
||||
queryType = 'X';*/
|
||||
Serial.print(APM_RC.InputCh(0));
|
||||
SerPri(APM_RC.InputCh(0));
|
||||
comma();
|
||||
Serial.print(ch_roll_slope);
|
||||
SerPri(ch_roll_slope);
|
||||
comma();
|
||||
Serial.print(ch_roll_offset);
|
||||
SerPri(ch_roll_offset);
|
||||
comma();
|
||||
Serial.println(ch_roll);
|
||||
SerPriln(ch_roll);
|
||||
break;
|
||||
case 'B': // Send roll, pitch and yaw PID values
|
||||
Serial.print(KP_QUAD_ROLL, 3);
|
||||
SerPri(KP_QUAD_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KI_QUAD_ROLL, 3);
|
||||
SerPri(KI_QUAD_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(STABLE_MODE_KP_RATE_ROLL, 3);
|
||||
SerPri(STABLE_MODE_KP_RATE_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KP_QUAD_PITCH, 3);
|
||||
SerPri(KP_QUAD_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KI_QUAD_PITCH, 3);
|
||||
SerPri(KI_QUAD_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(STABLE_MODE_KP_RATE_PITCH, 3);
|
||||
SerPri(STABLE_MODE_KP_RATE_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KP_QUAD_YAW, 3);
|
||||
SerPri(KP_QUAD_YAW, 3);
|
||||
comma();
|
||||
Serial.print(KI_QUAD_YAW, 3);
|
||||
SerPri(KI_QUAD_YAW, 3);
|
||||
comma();
|
||||
Serial.print(STABLE_MODE_KP_RATE_YAW, 3);
|
||||
SerPri(STABLE_MODE_KP_RATE_YAW, 3);
|
||||
comma();
|
||||
Serial.print(STABLE_MODE_KP_RATE, 3); // NOT USED NOW
|
||||
SerPri(STABLE_MODE_KP_RATE, 3); // NOT USED NOW
|
||||
comma();
|
||||
Serial.println(MAGNETOMETER, 3);
|
||||
SerPriln(MAGNETOMETER, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'D': // Send GPS PID
|
||||
Serial.print(KP_GPS_ROLL, 3);
|
||||
SerPri(KP_GPS_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KI_GPS_ROLL, 3);
|
||||
SerPri(KI_GPS_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KD_GPS_ROLL, 3);
|
||||
SerPri(KD_GPS_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KP_GPS_PITCH, 3);
|
||||
SerPri(KP_GPS_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KI_GPS_PITCH, 3);
|
||||
SerPri(KI_GPS_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KD_GPS_PITCH, 3);
|
||||
SerPri(KD_GPS_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(GPS_MAX_ANGLE, 3);
|
||||
SerPri(GPS_MAX_ANGLE, 3);
|
||||
comma();
|
||||
Serial.println(GEOG_CORRECTION_FACTOR, 3);
|
||||
SerPriln(GEOG_CORRECTION_FACTOR, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'F': // Send altitude PID
|
||||
Serial.print(KP_ALTITUDE, 3);
|
||||
SerPri(KP_ALTITUDE, 3);
|
||||
comma();
|
||||
Serial.print(KI_ALTITUDE, 3);
|
||||
SerPri(KI_ALTITUDE, 3);
|
||||
comma();
|
||||
Serial.println(KD_ALTITUDE, 3);
|
||||
SerPriln(KD_ALTITUDE, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'H': // Send drift correction PID
|
||||
Serial.print(Kp_ROLLPITCH, 4);
|
||||
SerPri(Kp_ROLLPITCH, 4);
|
||||
comma();
|
||||
Serial.print(Ki_ROLLPITCH, 7);
|
||||
SerPri(Ki_ROLLPITCH, 7);
|
||||
comma();
|
||||
Serial.print(Kp_YAW, 4);
|
||||
SerPri(Kp_YAW, 4);
|
||||
comma();
|
||||
Serial.println(Ki_YAW, 6);
|
||||
SerPriln(Ki_YAW, 6);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'J': // Send sensor offset
|
||||
Serial.print(gyro_offset_roll);
|
||||
SerPri(gyro_offset_roll);
|
||||
comma();
|
||||
Serial.print(gyro_offset_pitch);
|
||||
SerPri(gyro_offset_pitch);
|
||||
comma();
|
||||
Serial.print(gyro_offset_yaw);
|
||||
SerPri(gyro_offset_yaw);
|
||||
comma();
|
||||
Serial.print(acc_offset_x);
|
||||
SerPri(acc_offset_x);
|
||||
comma();
|
||||
Serial.print(acc_offset_y);
|
||||
SerPri(acc_offset_y);
|
||||
comma();
|
||||
Serial.println(acc_offset_z);
|
||||
SerPriln(acc_offset_z);
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
|
|
@ -235,136 +235,136 @@ void sendSerialTelemetry() {
|
|||
queryType = 'X';
|
||||
break;
|
||||
case 'P': // Send rate control PID
|
||||
Serial.print(Kp_RateRoll, 3);
|
||||
SerPri(Kp_RateRoll, 3);
|
||||
comma();
|
||||
Serial.print(Ki_RateRoll, 3);
|
||||
SerPri(Ki_RateRoll, 3);
|
||||
comma();
|
||||
Serial.print(Kd_RateRoll, 3);
|
||||
SerPri(Kd_RateRoll, 3);
|
||||
comma();
|
||||
Serial.print(Kp_RatePitch, 3);
|
||||
SerPri(Kp_RatePitch, 3);
|
||||
comma();
|
||||
Serial.print(Ki_RatePitch, 3);
|
||||
SerPri(Ki_RatePitch, 3);
|
||||
comma();
|
||||
Serial.print(Kd_RatePitch, 3);
|
||||
SerPri(Kd_RatePitch, 3);
|
||||
comma();
|
||||
Serial.print(Kp_RateYaw, 3);
|
||||
SerPri(Kp_RateYaw, 3);
|
||||
comma();
|
||||
Serial.print(Ki_RateYaw, 3);
|
||||
SerPri(Ki_RateYaw, 3);
|
||||
comma();
|
||||
Serial.print(Kd_RateYaw, 3);
|
||||
SerPri(Kd_RateYaw, 3);
|
||||
comma();
|
||||
Serial.println(xmitFactor, 3);
|
||||
SerPriln(xmitFactor, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'Q': // Send sensor data
|
||||
Serial.print(read_adc(0));
|
||||
SerPri(read_adc(0));
|
||||
comma();
|
||||
Serial.print(read_adc(1));
|
||||
SerPri(read_adc(1));
|
||||
comma();
|
||||
Serial.print(read_adc(2));
|
||||
SerPri(read_adc(2));
|
||||
comma();
|
||||
Serial.print(read_adc(4));
|
||||
SerPri(read_adc(4));
|
||||
comma();
|
||||
Serial.print(read_adc(3));
|
||||
SerPri(read_adc(3));
|
||||
comma();
|
||||
Serial.print(read_adc(5));
|
||||
SerPri(read_adc(5));
|
||||
comma();
|
||||
Serial.print(err_roll);
|
||||
SerPri(err_roll);
|
||||
comma();
|
||||
Serial.print(err_pitch);
|
||||
SerPri(err_pitch);
|
||||
comma();
|
||||
Serial.print(degrees(roll));
|
||||
SerPri(degrees(roll));
|
||||
comma();
|
||||
Serial.print(degrees(pitch));
|
||||
SerPri(degrees(pitch));
|
||||
comma();
|
||||
Serial.println(degrees(yaw));
|
||||
SerPriln(degrees(yaw));
|
||||
break;
|
||||
case 'R': // Send raw sensor data
|
||||
break;
|
||||
case 'S': // Send all flight data
|
||||
Serial.print(timer-timer_old);
|
||||
SerPri(timer-timer_old);
|
||||
comma();
|
||||
Serial.print(read_adc(0));
|
||||
SerPri(read_adc(0));
|
||||
comma();
|
||||
Serial.print(read_adc(1));
|
||||
SerPri(read_adc(1));
|
||||
comma();
|
||||
Serial.print(read_adc(2));
|
||||
SerPri(read_adc(2));
|
||||
comma();
|
||||
Serial.print(ch_throttle);
|
||||
SerPri(ch_throttle);
|
||||
comma();
|
||||
Serial.print(control_roll);
|
||||
SerPri(control_roll);
|
||||
comma();
|
||||
Serial.print(control_pitch);
|
||||
SerPri(control_pitch);
|
||||
comma();
|
||||
Serial.print(control_yaw);
|
||||
SerPri(control_yaw);
|
||||
comma();
|
||||
Serial.print(frontMotor); // Front Motor
|
||||
SerPri(frontMotor); // Front Motor
|
||||
comma();
|
||||
Serial.print(backMotor); // Back Motor
|
||||
SerPri(backMotor); // Back Motor
|
||||
comma();
|
||||
Serial.print(rightMotor); // Right Motor
|
||||
SerPri(rightMotor); // Right Motor
|
||||
comma();
|
||||
Serial.print(leftMotor); // Left Motor
|
||||
SerPri(leftMotor); // Left Motor
|
||||
comma();
|
||||
Serial.print(read_adc(4));
|
||||
SerPri(read_adc(4));
|
||||
comma();
|
||||
Serial.print(read_adc(3));
|
||||
SerPri(read_adc(3));
|
||||
comma();
|
||||
Serial.println(read_adc(5));
|
||||
SerPriln(read_adc(5));
|
||||
break;
|
||||
case 'T': // Spare
|
||||
break;
|
||||
case 'U': // Send receiver values
|
||||
Serial.print(ch_roll); // Aileron
|
||||
SerPri(ch_roll); // Aileron
|
||||
comma();
|
||||
Serial.print(ch_pitch); // Elevator
|
||||
SerPri(ch_pitch); // Elevator
|
||||
comma();
|
||||
Serial.print(ch_yaw); // Yaw
|
||||
SerPri(ch_yaw); // Yaw
|
||||
comma();
|
||||
Serial.print(ch_throttle); // Throttle
|
||||
SerPri(ch_throttle); // Throttle
|
||||
comma();
|
||||
Serial.print(ch_aux); // AUX1 (Mode)
|
||||
SerPri(ch_aux); // AUX1 (Mode)
|
||||
comma();
|
||||
Serial.print(ch_aux2); // AUX2
|
||||
SerPri(ch_aux2); // AUX2
|
||||
comma();
|
||||
Serial.print(roll_mid); // Roll MID value
|
||||
SerPri(roll_mid); // Roll MID value
|
||||
comma();
|
||||
Serial.print(pitch_mid); // Pitch MID value
|
||||
SerPri(pitch_mid); // Pitch MID value
|
||||
comma();
|
||||
Serial.println(yaw_mid); // Yaw MID Value
|
||||
SerPriln(yaw_mid); // Yaw MID Value
|
||||
break;
|
||||
case 'V': // Spare
|
||||
break;
|
||||
case 'X': // Stop sending messages
|
||||
break;
|
||||
case '!': // Send flight software version
|
||||
Serial.println(VER);
|
||||
SerPriln(VER);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case '2': // Send transmitter calibration values
|
||||
Serial.print(ch_roll_slope);
|
||||
SerPri(ch_roll_slope);
|
||||
comma();
|
||||
Serial.print(ch_roll_offset);
|
||||
SerPri(ch_roll_offset);
|
||||
comma();
|
||||
Serial.print(ch_pitch_slope);
|
||||
SerPri(ch_pitch_slope);
|
||||
comma();
|
||||
Serial.print(ch_pitch_offset);
|
||||
SerPri(ch_pitch_offset);
|
||||
comma();
|
||||
Serial.print(ch_yaw_slope);
|
||||
SerPri(ch_yaw_slope);
|
||||
comma();
|
||||
Serial.print(ch_yaw_offset);
|
||||
SerPri(ch_yaw_offset);
|
||||
comma();
|
||||
Serial.print(ch_throttle_slope);
|
||||
SerPri(ch_throttle_slope);
|
||||
comma();
|
||||
Serial.print(ch_throttle_offset);
|
||||
SerPri(ch_throttle_offset);
|
||||
comma();
|
||||
Serial.print(ch_aux_slope);
|
||||
SerPri(ch_aux_slope);
|
||||
comma();
|
||||
Serial.print(ch_aux_offset);
|
||||
SerPri(ch_aux_offset);
|
||||
comma();
|
||||
Serial.print(ch_aux2_slope);
|
||||
SerPri(ch_aux2_slope);
|
||||
comma();
|
||||
Serial.println(ch_aux2_offset);
|
||||
SerPriln(ch_aux2_offset);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case '.': // Modify GPS settings
|
||||
|
|
@ -380,16 +380,16 @@ float readFloatSerial() {
|
|||
char data[128] = "";
|
||||
|
||||
do {
|
||||
if (Serial.available() == 0) {
|
||||
if (SerAva() == 0) {
|
||||
delay(10);
|
||||
timeout++;
|
||||
}
|
||||
else {
|
||||
data[index] = Serial.read();
|
||||
data[index] = SerRea();
|
||||
timeout = 0;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
|
||||
return atof(data);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue