lhiggs CSUM
Capstone project for Bachelor's in Mechanical Engineering 2011
Dependencies: FatFileSystem MAX3100 MODGPS MODSERIAL SDFileSystem mbed
main.cpp
- Committer:
- lhiggs
- Date:
- 2013-05-29
- Revision:
- 0:0529d2d7762f
File content as of revision 0:0529d2d7762f:
#include "mbed.h" // MBED LIBRARY
#include "SerialRPCInterface.h" // MBED RPC LIBRARY (FOR LABVIEW CONTROL OF PROGRAM FUNCTIONS AND VARIABLES)
#include "GPS.h" // MBED GPS LIBRARY
#include "MODSERIAL.h" // MBED BUFFERED SERIAL
#include "SDFileSystem.h" // MBED SD LIBRARY
#include "UM6_usart.h" // UM6 USART HEADER
#include "UM6_config.h" // UM6 CONFIG HEADER
#include "MAX3100.h" // MAX3100 SPI to UART CHIP DRIVER LIBRARY
#include "math.h"
/////////////////////////////////////////////////////////////////////////////////////////////
// Program Name: main.cpp
// Description: This is the main program for an autonomous parafoil undergraduate capstone
// project in Mechanical Engineering at the California Maritime Academy, Vallejo
// CA, Spring 2011. As of yet there is no autonomous functionality. However, all
// hardware has proven functional. The hardware that this program was written for
// consists of an mbed LPC1768 microcontroller, cool components mbed breakout board
// with 1GB microSD card, GM862-GPS combined cellular and gps module, DNT900P
// 900Mhz 1Watt RF modem utilized as a transparent serial link to the application,
// Hitec HS-785 Winch Servos, Firgelli PQ12 linear actuator with limit switches,
// and last but by no means least a CH Robotics UM6 IMU/AHRS.
// Author: Logan Higgs, (portions of UM6 interface adapted from open source UM6 code by CH
// robotics}
// Date: 04/2011
// Special Thanks to Andy Kirkham from mbed who wrote most of the mbed libraries that this
// program utilizes, and more importantly even edited his MODGPS library to support VTG GPS
// data specifically from my request.
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS FOR RECIEVED DATA COUNTING AND BOOLING GOT NEW DATA FLAGS
/////////////////////////////////////////////////////////////////////////////////////////////
static uint8_t got_data_counter = 0;
volatile bool got_data = false;
volatile bool GOT_UM6 = false;
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS FOR ATTEMPT TO MAKE UM6 DATA ARRAYS FOR LETTING microSD CARD
// DATA WRITING ONLY ONCE A SECOND, INSTEAD OF EVERY TIME A UM6 DATA
// SET IS RECIEVED
/////////////////////////////////////////////////////////////////////////////////////////////
int UM6_data_counter = 0; // Counter for SD data logging
int16_t MY_DATA_GYRO_PROC_X;
int16_t MY_DATA_GYRO_PROC_Y;
int16_t MY_DATA_GYRO_PROC_Z;
int16_t MY_DATA_ACCEL_PROC_X;
int16_t MY_DATA_ACCEL_PROC_Y;
int16_t MY_DATA_ACCEL_PROC_Z;
int16_t MY_DATA_MAG_PROC_X;
int16_t MY_DATA_MAG_PROC_Y;
int16_t MY_DATA_MAG_PROC_Z;
int16_t MY_DATA_EULER_PHI;
int16_t MY_DATA_EULER_THETA;
int16_t MY_DATA_EULER_PSI;
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) SERIAL COMMUNICATION PINS ON MBED
/////////////////////////////////////////////////////////////////////////////////////////////
//MODSERIAL pc(USBTX, USBRX); // PC SERIAL OVER USB PORT ON MBED
//Create the interface on the USB Serial Port
//SerialRPCInterface SerialInterface(USBTX, USBRX);
SerialRPCInterface SerialInterface(p28, p27);
MODSERIAL uart(p26, p25); // UM6 SERIAL OVER UART PINS 28 & 27
// SETUP DNT900P RF MODEM SERIAL
//MODSERIAL DNT900P(p28, p27);
// SETUP GM862-GPS MODEM SERIAL PORT THRU MAX3100 SPI to UART IC
MAX3100 max(p11, p12, p13, p14,p15);
// SETUP GM862-GPS GPS SERIAL PORT
// Serial gps_uart(p9, p10);
// Create an instance of the GPS object for MODGPS library
GPS gps(NC, p10);
////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) SPI COMMUNICATION PINS ON MBED FOR SD CARD ON COOLCOMPONENTS WORKSHOP BOARD
////////////////////////////////////////////////////////////////////////////////////////////////
SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) WINCH SERVO#1 (STBD) & WINCH SERVO#2 (PORT)
////////////////////////////////////////////////////////////////////////////////////////////////
PwmOut servo1(p23);
PwmOut servo2(p24);
////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) DIGITAL OUTPUTS FOR H-BRIDGE ACTUATOR DRIVER FOR PARAFOIL DEPLOYMENT
////////////////////////////////////////////////////////////////////////////////////////////////
DigitalOut red(p16);
DigitalOut EN(p17);
DigitalOut blue(p18);
////////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) MBED LED (1 thru 3) FOR VISUAL DEBUGGING ON MBED
////////////////////////////////////////////////////////////////////////////////////////////////
DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
DigitalOut sd_activity(LED3); // LED3 = SD CARD
// CREATE FLOAT VARIABLES FOR LABVIEW REMOTE CONTROL OF PROGRAM
float servo1_pw = 0.0015;
float servo2_pw = 0.0015;
float Actuator_CMD = 1;
float Latitude = 0;
float Longitude = 0;
float Altitude = 0;
float Roll = 0;
float Pitch = 0;
float Yaw = 0;
float Control_Mode = 0;
float CMD_Latitude = 0;
float CMD_Longitude = 0;
float CALC_Heading = 0;
float CMD_Heading = 0;
// FROM UM6 FIRMWARE SOURCE
// Flag for storing the current USART state
uint8_t gUSART_State = USART_STATE_WAIT;
/*******************************************************************************
* Function Name : ComputeChecksum
* Input : USARTPacket* new_packet
* Output : None
* Return : uint16_t
* Description : Returns the two byte sum of all the individual bytes in the
given packet.
*******************************************************************************/
uint16_t ComputeChecksum( USARTPacket* new_packet ) {
int32_t index;
uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
for ( index = 0; index < new_packet->data_length; index++ ) {
checksum += new_packet->packet_data[index];
}
return checksum;
}
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
// MODSERIAL INTERUPT FUNCTION AND MODSERIAL OVERFLOW INTERUPT FUNCTION FOR DEBUGGING
/////////////////////////////////////////////////////////////////////////////////////////////
// MBED MODSERIAL INTERUPT FUNCTION
// This function is called when a character goes into the RX buffer.
//void rxCallback(void) {
// if (uart.rxBufferGetCount() > MAX_PACKET_DATA & got_data == false) {
// got_data = true;
// uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
// }
//}
void rxOvflow(void) {
error("Ouch, overflowed");
}
//SDFileSystem *sd;
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
int main() {
///////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////
//Make these variables accessible over RPC by attaching them to an RPCVariable
RPCVariable<float> RPCservo1_pw(&servo1_pw, "servo1_pw");
RPCVariable<float> RPCservo2_pw(&servo2_pw, "servo2_pw");
RPCVariable<float> RPCActuator_CMD(&Actuator_CMD, "Actuator_CMD");
RPCVariable<float> RPCLatitude(&Latitude, "Latitude");
RPCVariable<float> RPCLongitude(&Longitude, "Longitude");
RPCVariable<float> RPCAltitude(&Altitude, "Altitude");
RPCVariable<float> RPCRoll(&Roll, "Roll");
RPCVariable<float> RPCPitch(&Pitch, "Pitch");
RPCVariable<float> RPCYaw(&Yaw, "Yaw");
RPCVariable<float> RPCControl_Mode(&Control_Mode, "Control_Mode");
RPCVariable<float> RPCCMD_Latitude(&CMD_Latitude, "CMD_Latitude");
RPCVariable<float> RPCCMD_Longitude(&CMD_Longitude, "CMD_Lognitude");
RPCVariable<float> RPCCALC_Heading(&CALC_Heading, "CALC_Heading");
RPCVariable<float> RPCCMD_Heading(&CMD_Heading, "CMD_Heading");
///////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
GPS_Time q1;
GPS_VTG v1;
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
// WINCH SERVO INITIALIZATION
//////////////////////////////////////////////////////////////////////////////////////////////////////
servo1.period(0.020); // servos requires a 20ms period
servo2.period(0.020);
// initialize servo to center (neutral position)
servo1.pulsewidth(0.0015);
servo2.pulsewidth(0.0015);
//servo_neutral_activity = !servo_neutral_activity;
///////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS MAKES A DIRECTORY ON THE microSD CARD FOR DATA LOGGING
//////////////////////////////////////////////////////////////////////////////////////////////////////
// SD DATA LOGGING
// mkdir("/sd/", 0777);
// sd_activity = !sd_activity;
//////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
// SET SERIAL UART BAUD RATES
/////////////////////////////////////////////////////////////////////////////////////////////////////
// set UM6 serial uart baud 115200
uart.baud(9600);
// pc bauds for usb virtual serial port on mbed for ground use only.
// pc.baud(9600); // pc baud for GM862-GPS modem to pc interface
// pc.baud(9600); // pc baud for UM6 to pc interface
// set DNT900P baud 9600
// DNT900P.baud(9600);
// set gps baud 4800
gps.baud(4800);
gps.format(8, GPS::None, 1);
// enable max3100 interupts for GM862-GPS modem uart thru spi to uart ic
max.enableRxIrq();
max.enableTxIrq();
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS WAS FOR DEBUGGING MODSERIAL AND SD CARD, STILL USEFUL FOR CHECKING FOR OVERFLOW OF MODSERIAL
// CIRCULAR BUFFER
/////////////////////////////////////////////////////////////////////////////////////////////////////
// In main after setting the uart baud rate and before attaching your rxCallback function add:-
uart.attach(&rxOvflow, MODSERIAL::RxOvIrq);
// Setup the uart serial port, then...
// sd = new SDFileSystem(p5, p6, p7, p8, "sd");
// attach interupt function to uart
// uart.attach(&rxCallback, MODSERIAL::RxIrq);
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
while (1) {
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// GPS AUTOMATIC DATA PARSING WITH MODGPS LIBRARY
// Demonstrate how to find out the GPS location co-ords.
// pc.printf("Lat = %.4f ", gps.latitude());
// pc.printf("Lon = %.4f ", gps.longitude());
// pc.printf("Alt = %.4f ", gps.altitude());
// Gran a snapshot of the current time.
gps.timeNow(&q1);
// pc.printf("%02d:%02d:%02d %02d/%02d/%04d\r\n",
// q1.hour, q1.minute, q1.second, q1.day, q1.month, q1.year);
gps.vtg(&v1);
// pc.printf("Method 1. Vector data, Speed (knots):%lf, Speed(kph):%lf, Track(true):%lf, Track(mag)%lf\r\n",
// v1.velocity_knots(), v1.velocity_kph(), v1.track_true(), v1.track_mag());
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS A SERIAL PASS THRU BETWEEN THE GM862-GPS MODEM SERIAL PORT AND THE PC USB VIRTUAL SERIAL PORT.
// IT ISN'T INTENDED TO BE USED DURING FLIGHT, BUT RATHER FOR CONFIGURATION OF THE GM862-GPS WHILE ON
// THE GROUND.
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// if (pc.readable()) {
// int c = pc.getc();
// max.putc(c);
// }
// if (max.readable()) {
// pc.putc( max.getc() );
// }
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS A SERIAL PASS THRU FOR THE UM6 AHRS/IMU TO THE PC FOR USING THE CH ROBOTICS SERIAL INTERFACE
// PC SOFTWARE FOR CALIBRATING AND VIEWING DATA FROM THE UM6. DEFAULT PC BAUD 115200
// if (pc.readable()) {
// int c = pc.getc();
// uart.putc(c);
// }
// if (uart.readable()) {
// pc.putc( uart.getc() );
// }
/////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS FOR THE UM6 IMU/AHRS SERIAL COMMUNICATIONS, IT REALLY SHOULD BE PUT INTO AT LEAST ITS OWN
// FUNCTIONS, OR EVEN A UM6 CLASS AND LIBRARY.
///////////////////////////////////////////////////////////////////////////////////////////////////////
if (uart.rxBufferGetCount() > MAX_PACKET_DATA) {
uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
// UM6 Firmware Source Function ProcessNextCharacter()
//void ProcessNextCharacter( ) {
static uint8_t data_counter = 0;
static USARTPacket new_packet;
// The next action should depend on the USART state.
switch ( gUSART_State ) {
// USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
// that signals a new incoming packet.
case USART_STATE_WAIT:
if ( data_counter == 0 ) { // Waiting on 's' character
if ( uart.getc() == 's' ) {
data_counter++;
} else {
data_counter = 0;
}
} else if ( data_counter == 1 ) { // Waiting on 'n' character
if ( uart.getc() == 'n' ) {
data_counter++;
} else {
data_counter = 0;
}
} else if ( data_counter == 2 ) { // Waiting on 'p' character
if ( uart.getc() == 'p' ) {
// The full 'snp' sequence was received. Reset data_counter (it will be used again
// later) and transition to the next state.
data_counter = 0;
gUSART_State = USART_STATE_TYPE;
} else {
data_counter = 0;
}
}
break;
// USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
// indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
case USART_STATE_TYPE:
new_packet.PT = uart.getc();
gUSART_State = USART_STATE_ADDRESS;
break;
// USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
// the address that the packet applies to
case USART_STATE_ADDRESS:
new_packet.address = uart.getc();
// pc.putc(new_packet.address);
// For convenience, identify the type of packet this is and copy to the packet structure
// (this will be used by the packet handler later)
if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
new_packet.address_type = ADDRESS_TYPE_CONFIG;
} else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
new_packet.address_type = ADDRESS_TYPE_DATA;
} else {
new_packet.address_type = ADDRESS_TYPE_COMMAND;
}
// Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
// If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet
if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
gUSART_State = USART_STATE_CHECKSUM;
}
// If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
else {
gUSART_State = USART_STATE_DATA;
// Determine the expected number of bytes in this data packet based on the packet type. A write operation
// consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
// where the batch size is also given in the packet type byte.
if ( new_packet.PT & PACKET_IS_BATCH ) {
new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
} else {
new_packet.data_length = 4;
}
}
break;
// USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
// data.
case USART_STATE_DATA:
new_packet.packet_data[data_counter] = uart.getc();
data_counter++;
// If the expected number of bytes has been received, transition to the CHECKSUM state.
if ( data_counter == new_packet.data_length ) {
// Reset data_counter, since it will be used in the CHECKSUM state.
data_counter = 0;
gUSART_State = USART_STATE_CHECKSUM;
}
break;
// USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
// of the 16-bit checksum.
case USART_STATE_CHECKSUM:
// Get the highest-order byte
if ( data_counter == 0 ) {
// uint8_t Temp_Packet = uart.getc();
new_packet.checksum = ((uint16_t)uart.getc() << 8);
data_counter++;
} else { // ( data_counter == 1 )
// Get lower-order byte
new_packet.checksum = new_packet.checksum | ((uint16_t)uart.getc() & 0x0FF);
// Both checksum bytes have been received. Make sure that the checksum is valid.
uint16_t checksum = ComputeChecksum( &new_packet );
// If checksum does not match, send a BAD_CHECKSUM packet
if ( checksum != new_packet.checksum ) {
got_data = false;
}
else
{
// Packet was received correctly.
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//
// ADDED: CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
// IF DATA ADDRESS
if (new_packet.address_type == ADDRESS_TYPE_DATA) {
//------------------------------------------------------------
// UM6_GYRO_PROC_XY (0x5C)
// To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
// per second, the data should be multiplied by the scale factor 0.0610352 as shown below
// angular_rate = register_data*0.0610352
if (new_packet.address == UM6_GYRO_PROC_XY) {
// GYRO_PROC_X
MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
// pc.printf( "GPx %f deg/s\n",MY_DATA_GYRO_PROC_X*0.0610352);
MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
// pc.printf( "GPy %f d/s\n",MY_DATA_GYRO_PROC_Y*0.0610352);
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_GYRO_PROC_Z (0x5D)
// To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
// degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
// GYRO_PROC_Z
MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];
//pc.printf( "GPz %f deg/s\n",MY_DATA_GYRO_PROC_Z*0.0610352);
}
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_ACCEL_PROC_XY (0x5E)
// To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
// the data should be multiplied by the scale factor 0.000183105 as shown below.
// acceleration = register_data* 0.000183105
if (new_packet.address == UM6_ACCEL_PROC_XY) {
// ACCEL_PROC_X
MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
// pc.printf( "Apx %f g \n",MY_DATA_ACCEL_PROC_X*0.000183105);
// ACCEL_PROC_Y
MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
// pc.printf( "Apy %f g \n",MY_DATA_ACCEL_PROC_Y*0.000183105);
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_ACCEL_PROC_Z (0x5F)
// To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
// gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
// ACCEL_PROC_Z
MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
// pc.printf( "Apz %f g \n",MY_DATA_ACCEL_PROC_Z*0.000183105);
}
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_MAG_PROC_XY (0x60)
// To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
// calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
// shown below.
// magnetic field = register_data* 0.000305176
if (new_packet.address == UM6_MAG_PROC_XY) {
// MAG_PROC_X
MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
// pc.printf( "Mpx %f \n",MY_DATA_MAG_PROC_X*0.000305176);
// MAG_PROC_Y
MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
// pc.printf( "Mpy %f \n",MY_DATA_MAG_PROC_Y*0.000305176);
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_MAG_PROC_Z (0x61)
// To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
// calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
// shown below.
// magnetic field = register_data*0.000305176
// MAG_PROC_Z
MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
// pc.printf( "Mpz %f \n",MY_DATA_MAG_PROC_Z*0.000305176);
}
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_EULER_PHI_THETA (0x62)
// Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
// estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
// degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
// angle estimate = register_data* 0.0109863
if (new_packet.address == UM6_EULER_PHI_THETA) {
// EULER_PHI (ROLL)
MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
MY_DATA_EULER_PHI |= new_packet.packet_data[1];
// pc.printf( "PHI %f deg , ",MY_DATA_EULER_PHI*0.0109863);
// EULER_THETA (PITCH)
MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
MY_DATA_EULER_THETA |= new_packet.packet_data[3];
// printf( "THETA %f deg , ",MY_DATA_EULER_THETA*0.0109863);
//------------------------------------------------------------
//------------------------------------------------------------
// UM6_EULER_PSI (0x63) (YAW)
// Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
// bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
// should be multiplied by the scale factor 0.0109863 as shown below
MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
MY_DATA_EULER_PSI |= new_packet.packet_data[5];
// pc.printf( "PSI %f deg \r\n",MY_DATA_EULER_PSI*0.0109863);
GOT_UM6 = true;
UM6_data_counter++;
}
//------------------------------------------------------------
}
// A full packet has been received.
// Put the USART back into the WAIT state and reset
// the data_counter variable so that it can be used to receive the next packet.
data_counter = 0;
gUSART_State = USART_STATE_WAIT;
// ADDED: This is a counter for the SD data logging
got_data_counter ++;
got_data = false;
}
}
break;
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// LABVIEW REMOTE CONTROL ENABLE
if (Control_Mode >= 0.5) {
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// LABVIEW REMOTE CONTROL OF SERVOS AND ACTUATOR
servo1.pulsewidth(servo1_pw); // Turn STBD
servo2.pulsewidth(servo2_pw);
// ACTUATOR RETRACT
if (Actuator_CMD < 0.5) {
red = 1;
EN = 1;
blue = 0;
}
// ACTUATOR EXTEND
if (Actuator_CMD >= 0.5) {
red = 0;
EN = 1;
blue = 1;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS A SIMPLE TEST FOR CONTROLLING THE WINCH SERVOS FROM THE FLIGHT YAW ANGLE ESTIMATED FROM THE
// ONBOARD UM6 IMU/AHRS.
//////////////////////////////////////////////////////////////////////////////////////////////////////////
if (GOT_UM6 == true) {
// if (MY_DATA_EULER_PSI*0.0109863 < 5 && MY_DATA_EULER_PSI*0.0109863 > -5) {
if (MY_DATA_EULER_PSI*0.0109863 < CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 > CALC_Heading-5) {
servo1.pulsewidth(0.0015); // NEUTRAL STBD
servo2.pulsewidth(0.0015); // NEUTRAL PORT
}
if (MY_DATA_EULER_PSI*0.0109863 <= CALC_Heading-5 && MY_DATA_EULER_PSI*0.0109863 >= -135) {
servo1.pulsewidth(0.0015 + 0.0002); // Turn STBD
servo2.pulsewidth(0.0015);
// servo1.pulsewidth(0.0011);
// servo2.pulsewidth(0.0015 + 0.0002);
//servo_cw_activity = !servo_cw_activity;
}
if (MY_DATA_EULER_PSI*0.0109863 >= CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 <= 135) {
servo1.pulsewidth(0.0015);
servo2.pulsewidth(0.0015 - 0.0002); // Turn PORT
//servo_ccw_activity = !servo_ccw_activity;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
// DEPLOY PARAFOIL (RETRACT ACTUATOR FOR DEMONSTRATION PURPOSES ONLY WHEN PITCH ABOVE 85 AND BELOW 95 DEG.
if (MY_DATA_EULER_THETA*0.0109863 > 75 ) {
red = 1; // RETRACTS DEPLOYMENT ACTUATOR
EN = 1;
blue = 0;
}
if (MY_DATA_EULER_THETA*0.0109863 <= -75) {
red = 0; // RETRACTS DEPLOYMENT ACTUATOR
EN = 1;
blue = 1;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////
GOT_UM6 = false;
} // end if for GOT_EULER == true
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS A SIMPLE DATA OUTPUT USED TO TEST BASIC FUNCTIONALITY OF RF MODEM
/////////////////////////////////////////////////////////////////////////////////////////////////
if (UM6_data_counter >= 20) {
UM6_data_counter = 0;
Roll = MY_DATA_EULER_PHI*0.0109863;
Pitch = MY_DATA_EULER_THETA*0.0109863;
Yaw = MY_DATA_EULER_PSI*0.0109863;
Latitude = gps.latitude();
Longitude = gps.longitude();
Altitude = gps.altitude();
CALC_Heading = atan2 (cos(Latitude)*sin(CMD_Latitude)-sin(Latitude)*cos(CMD_Latitude)*cos(CMD_Longitude-CMD_Longitude),sin(CMD_Longitude-Longitude)*cos(CMD_Latitude)) * 180 / 3.14159265;
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
// THIS IS FOR microSD CARD FLIGHT DATA LOGGING, NEEDS TO BE OPTIMIZED AND EXPANDED TO LOG ALL DATA AND
// RECIEVED FLIGHT COMMANDS.
//////////////////////////////////////////////////////////////////////////////////////////////////////////
// FILE *fp = fopen("/sd/UM6_DATA_GROUND.txt", "a");
// if (fp == NULL) {
// error("Could not open file for write\n");
// }
// fprintf(fp,"%02d:%02d:%02d %02d/%02d/%04d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",
// q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
// gps.latitude(),gps.longitude(),gps.altitude(),
// v1.velocity_knots(),v1.track_true(),
// MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
// MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
// MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
// MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
// fclose(fp);
sd_activity = !sd_activity;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This sends the GPS:lat,long,alt telemetery to the ground base station
// with the DNT900 900Mhz 1Watt RF modem.
// DNT900P.printf("%02d:%02d:%02d %02d/%02d/%04d,\r\nLat %f,Long %f,Alt %f,Roll %f,Pitch %f,Yaw %f \n\r",
// q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
// gps.latitude(),gps.longitude(),gps.altitude(),
// MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
// DNT900P.printf("%02d:%02d:%02d %02d/%02d/%04d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",
// q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
// gps.latitude(),gps.longitude(),gps.altitude(),
// v1.velocity_knots(),v1.track_true(),
// MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
// MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
// MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
// MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
} // end UM6_data_counter >= 20 if statement
/////////////////////////////////////////////////////////////////////////////////////////////////
} // end while(1) loop
} // end main()