Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Xbee/Ser0 telemetry ports are working again

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@682 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-10-17 17:34:17 +00:00
parent 455e315a1a
commit 35b754bbae
4 changed files with 199 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArducopterNG/Arducopter.h
View File

@ -75,7 +75,8 @@ TODO:
#define SERIAL2_BAUD 115200
#define SERIAL3_BAUD 115200
#ifdef SerXbee // Xbee/Telemetry port
#ifdef SerXbee // Xbee/Telemetry port
//#define SerBau 115200 // Baud setting moved close next to port selection
#define SerPri Serial3.print
#define SerPrln Serial3.println
#define SerRea Serial3.read
@ -86,11 +87,8 @@ TODO:
#define SerPor "FTDI"
#endif
#ifndef SerPri // If we don't have SerPri set yet, it means we have are using Serial0
#define SerUSB
#endif
#ifdef SerUSB // Defines for normal Serial0 port
#ifndef SerXbee // If we don't have SerXbee set, it means we have are using Serial0
//#define SerBau 115200 // Baud setting moved close next to port selection
#define SerPri Serial.print
#define SerPrln Serial.println
#define SerRea Serial.read

199
ArducopterNG/ArducopterNG.pde
View File

@ -6,9 +6,9 @@
File : ArducopterNG.pde
Version : v1.0, 11 October 2010
Author(s): ArduCopter Team
Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -22,7 +22,7 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
/* ********************************************************************** */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* Mounting position : RC connectors pointing backwards */
@ -45,25 +45,41 @@
//#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 IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
//LEGACY-jp #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 UseAirspeed
//#define UseBMP
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!)
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
#define CONFIGURATOR
////////////////////
// Telemetry
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link
// If we are using normal FTDI/USB port as our telemetry/configuration, comment out next line
// If we are using normal FTDI/USB port as our telemetry/configuration, disable next
//#define SerXbee
//#define Ser0 // FTDI/USB Port Either one
//#define Ser3 // Telemetry port
// Telemetry port speed, default is 115200
#define SerBau 115200
//////////////////////
// Flight orientation
// Frame build condiguration
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
/* ************************************************************ */
/* **************** MAIN PROGRAM - INCLUDES ******************* */
/* ************************************************************ */
@ -92,6 +108,7 @@
/* Software version */
#define VER 1.5 // Current software version (only numeric values)
/* ************************************************************ */
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
/* ************************************************************ */
@ -107,9 +124,9 @@ unsigned long slowLoop = 0;
/* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */
void setup() {
APM_Init(); // APM Hardware initialization (in System.pde)
mainLoop = millis(); // Initialize timers
mediumLoop = mainLoop;
GPS_timer = mainLoop;
@ -146,10 +163,10 @@ void loop()
//float aux_float;
currentTime = millis();
// Main loop at 200Hz (IMU + control)
if ((currentTime-mainLoop) > 5) // 200Hz (every 5ms)
{
{
G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!!
mainLoop = currentTime;
@ -170,31 +187,31 @@ void loop()
if (AP_mode == AP_NORMAL_MODE) // Normal mode
Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
else // Automatic mode : GPS position hold mode
Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
}
Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
}
else { // ACRO Mode
gled_speed = 400;
Rate_control_v2();
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction
command_rx_yaw = ToDeg(yaw);
}
}
// Send output commands to motor ESCs...
motor_output();
// Autopilot mode functions
if (AP_mode == AP_AUTOMATIC_MODE)
{
{
if (target_position)
{
{
if (GPS.NewData) // New GPS info?
{
{
read_GPS_data();
Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
}
}
}
else // First time we enter in GPS position hold we capture the target position as the actual position
{
{
if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude;
@ -203,68 +220,90 @@ void loop()
target_baro_altitude = press_alt;
Initial_Throttle = ch_throttle;
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
}
}
command_gps_roll=0;
command_gps_pitch=0;
}
}
}
else
target_position=0;
}
// Medium loop (about 60Hz)
if ((currentTime-mediumLoop)>=17){
mediumLoop = currentTime;
GPS.Read(); // Read GPS data
// Each of the six cases executes at 10Hz
switch (medium_loopCounter){
case 0: // Magnetometer reading (10Hz)
medium_loopCounter++;
slowLoop++;
#ifdef IsMAG
if (MAGNETOMETER == 1) {
APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading
}
#endif
break;
case 1: // Barometer reading (2x10Hz = 20Hz)
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
Baro_new_data = 1;
}
#endif
break;
case 2: // Send serial telemetry (10Hz)
medium_loopCounter++;
#ifdef CONFIGURATOR
sendSerialTelemetry();
#endif
break;
case 3: // Read serial telemetry (10Hz)
medium_loopCounter++;
#ifdef CONFIGURATOR
readSerialCommand();
#endif
break;
case 4: // second Barometer reading (2x10Hz = 20Hz)
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
Baro_new_data = 1;
}
#endif
break;
case 5: // Battery monitor (10Hz)
medium_loopCounter=0;
#if BATTERY_EVENT == 1
read_battery(); // Battery monitor
#endif
break;
}
// Medium loop (about 60Hz)
if ((currentTime-mediumLoop)>=17){
mediumLoop = currentTime;
GPS.Read(); // Read GPS data
// Each of the six cases executes at 10Hz
switch (medium_loopCounter){
case 0: // Magnetometer reading (10Hz)
medium_loopCounter++;
slowLoop++;
#ifdef IsMAG
if (MAGNETOMETER == 1) {
APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading
}
#endif
break;
case 1: // Barometer reading (2x10Hz = 20Hz)
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
Baro_new_data = 1;
}
#endif
break;
case 2: // Send serial telemetry (10Hz)
medium_loopCounter++;
#ifdef CONFIGURATOR
sendSerialTelemetry();
#endif
break;
case 3: // Read serial telemetry (10Hz)
medium_loopCounter++;
#ifdef CONFIGURATOR
readSerialCommand();
#endif
break;
case 4: // second Barometer reading (2x10Hz = 20Hz)
medium_loopCounter++;
#ifdef UseBMP
if (APM_BMP085.Read()){
read_baro();
Baro_new_data = 1;
}
#endif
break;
case 5: // Battery monitor (10Hz)
medium_loopCounter=0;
#if BATTERY_EVENT == 1
read_battery(); // Battery monitor
#endif
break;
}
}
// AM and Mode status LED lights
if(millis() - gled_timer > gled_speed) {
gled_timer = millis();
if(gled_status == HIGH) {
digitalWrite(LED_Green, LOW);
#ifdef IsAM
digitalWrite(RE_LED, LOW);
#endif
gled_status = LOW;
// SerPrln("L");
}
else {
digitalWrite(LED_Green, HIGH);
#ifdef IsAM
if(motorArmed) digitalWrite(RE_LED, HIGH);
#endif
gled_status = HIGH;
}
}
}

119
ArducopterNG/GCS.pde
View File

@ -6,9 +6,9 @@
File : GCS.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -22,16 +22,16 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
//
// Function : send_message()
//
@ -43,10 +43,10 @@ TODO:
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{
if(severity >= DEBUG_LEVEL){
SerPri("MSG: ");
SerPrln(str);
}
if(severity >= DEBUG_LEVEL){
SerPri("MSG: ");
SerPrln(str);
}
}
@ -61,7 +61,7 @@ void send_message(byte severity, const char *str) // This is the instance of se
void readSerialCommand() {
// Check for serial message
if (SerAva()) {
queryType = SerRea();
switch (queryType) {
case 'A': // Stable PID
KP_QUAD_ROLL = readFloatSerial();
@ -106,14 +106,14 @@ void readSerialCommand() {
acc_offset_z = readFloatSerial();
break;
case 'K': // Spare
break;
break;
case 'M': // Receive debug motor commands
frontMotor = readFloatSerial();
backMotor = readFloatSerial();
rightMotor = readFloatSerial();
leftMotor = readFloatSerial();
motorArmed = readFloatSerial();
break;
break;
case 'O': // Rate Control PID
Kp_RateRoll = readFloatSerial();
Ki_RateRoll = readFloatSerial();
@ -126,7 +126,7 @@ void readSerialCommand() {
Kd_RateYaw = readFloatSerial();
xmitFactor = readFloatSerial();
break;
case 'W': // Write all user configurable values to EEPROM
case 'W': // Write all user configurable values to EEPROM
writeUserConfig();
break;
case 'Y': // Initialize EEPROM with default values
@ -145,7 +145,7 @@ void readSerialCommand() {
ch_aux_offset = readFloatSerial();
ch_aux2_slope = readFloatSerial();
ch_aux2_offset = readFloatSerial();
break;
break;
}
}
}
@ -154,34 +154,34 @@ void sendSerialTelemetry() {
float aux_float[3]; // used for sensor calibration
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
/* SerPri(("throttle =");
SerPrln(ch_throttle);
SerPri("control roll =");
SerPrln(control_roll-CHANN_CENTER);
SerPri("control pitch =");
SerPrln(control_pitch-CHANN_CENTER);
SerPri("control yaw =");
SerPrln(control_yaw-CHANN_CENTER);
SerPri("front left yaw =");
SerPrln(frontMotor);
SerPri("front right yaw =");
SerPrln(rightMotor);
SerPri("rear left yaw =");
SerPrln(leftMotor);
SerPri("rear right motor =");
SerPrln(backMotor);
SerPrln();
SerPri("current roll rate =");
SerPrln(read_adc(0));
SerPri("current pitch rate =");
SerPrln(read_adc(1));
SerPri("current yaw rate =");
SerPrln(read_adc(2));
SerPri("command rx yaw =");
SerPrln(command_rx_yaw);
SerPrln();
queryType = 'X';*/
/* SerPri(("throttle =");
SerPrln(ch_throttle);
SerPri("control roll =");
SerPrln(control_roll-CHANN_CENTER);
SerPri("control pitch =");
SerPrln(control_pitch-CHANN_CENTER);
SerPri("control yaw =");
SerPrln(control_yaw-CHANN_CENTER);
SerPri("front left yaw =");
SerPrln(frontMotor);
SerPri("front right yaw =");
SerPrln(rightMotor);
SerPri("rear left yaw =");
SerPrln(leftMotor);
SerPri("rear right motor =");
SerPrln(backMotor);
SerPrln();
SerPri("current roll rate =");
SerPrln(read_adc(0));
SerPri("current pitch rate =");
SerPrln(read_adc(1));
SerPri("current yaw rate =");
SerPrln(read_adc(2));
SerPri("command rx yaw =");
SerPrln(command_rx_yaw);
SerPrln();
queryType = 'X';*/
SerPri(APM_RC.InputCh(0));
comma();
SerPri(ch_roll_slope);
@ -268,7 +268,7 @@ void sendSerialTelemetry() {
queryType = 'X';
break;
case 'L': // Spare
// RadioCalibration();
// RadioCalibration();
queryType = 'X';
break;
case 'N': // Send magnetometer config
@ -351,6 +351,20 @@ void sendSerialTelemetry() {
SerPri(read_adc(3));
comma();
SerPrln(read_adc(5));
/* comma();
SerPri(APM_Compass.Heading, 4);
comma();
SerPri(APM_Compass.Heading_X, 4);
comma();
SerPri(APM_Compass.Heading_Y, 4);
comma();
SerPri(APM_Compass.Mag_X);
comma();
SerPri(APM_Compass.Mag_Y);
comma();
SerPri(APM_Compass.Mag_Z);
comma();
*/
break;
case 'T': // Spare
break;
@ -406,7 +420,7 @@ void sendSerialTelemetry() {
comma();
SerPrln(ch_aux2_offset);
queryType = 'X';
break;
break;
case '.': // Modify GPS settings, print directly to GPS Port
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
break;
@ -438,3 +452,4 @@ float readFloatSerial() {
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data);
}

12
ArducopterNG/System.pde
View File

@ -85,9 +85,14 @@ void APM_Init() {
DataFlash.StartWrite(1); // Start a write session on page 1
Serial.begin(115200);
//Serial.println("ArduCopter Quadcopter v1.0");
// Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10
//Serial.println("ArduCopter Quadcopter v1.0");
// Proper Serial port/baud are defined on main .pde and then Arducopter.h with
// Choises of Xbee or
SerBeg(SerBau);
// 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)
@ -126,5 +131,6 @@ void APM_Init() {
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
#endif
}