/// makes one attempt to write a change to rx mode
/// return 0 on success, -1 on failure after 1 ms timeout
int ZigBee_SetModeRx() {
uint8_t zigbee_state = ZigBee_ReadState();
// wait until current state permits a transition
while(zigbee_state == STATUS_BUSY_RX || zigbee_state == STATUS_BUSY_TX || zigbee_state == STATUS_STATE_TRANSITION) {
DelayMicroseconds(100); // should not be required
zigbee_state = ZigBee_ReadState();
}
// if current state is already STATUS_RX_ON, return success
if(zigbee_state == STATUS_RX_ON) {
return(0); // success
}
// request transition
ZigBee_WriteReg(REG_TRX_STATE, CMD_RX_ON);
// poll state until transition verified
// datasheet reports nominal <170 us transition time
// timeout and fail after 1 ms
for(int i = 0; i < 100; i++) { // TODO: revisit timeout threshold
zigbee_state = ZigBee_ReadState();
if (zigbee_state == STATUS_RX_ON)
return(0); // success
}
// zigbee failed to reach state STATUS_RX_ON
return(-1); // failure
}
int ZigBee_CheckPartAndVers()
{
uint8_t part, vers;
int ret = -1;
part = ZigBee_GetPartNum();
DelayMicroseconds(1);
vers = ZigBee_GetVersionNum();
#if defined(DEBUG)
printf("got part num %d and version num %d\r\n",part,vers);
#endif
if ( (part == 3) && (vers == 2) )
{
ret = 0;
#if defined(DEBUG)
printf("found 2.4Ghz zigbee\r\n");
#endif
}
else if ( (part == 7) && ((vers == 1) || (vers == 3)))
{
ret = 0;
#if defined(DEBUG)
printf("found 900Mhz zigbee\r\n");
#endif
}
return ret;
}
int ZigBee_ConfigClock()
{
int ret = 0;
//2mA all pins, CLKM disabled
ZigBee_WriteReg(REG_TRX_CTRL_0,0x00);
DelayMicroseconds(2);
// Internal crystal disabled, use external freq input
ZigBee_WriteReg(REG_XOSC_CTRL,0x40);
return ret;
}
void AquireImage() // Read TSL1401 chip into sample array
{
uint8 i;
uint16 d;
asm("di"); // timing critical area, disable any interrupts!
BACKLIGHT(ON);
// 1. clock out crap as fast as possible (see TSL1401 documentation or parallax TLS1401 developer board is better) 2uS x 128 = 256uS
SI(ON);
for(i=0;i<129;i++)
{
CLK(ON);
DelayMicroseconds(1);
SI(OFF);
CLK(OFF);
DelayMicroseconds(1);
if(i>exposure) BACKLIGHT(OFF);
}
// 2. Wait for exposure to finish if its greater than 128
if(exposure>128) DelayMicroseconds(exposure-128);
BACKLIGHT(OFF); // turn off the backlight
// 3. Second pass, clock out the real data into raw buffer for processing and also average samples and put in sample array
SI(ON);
asm("ei");
for(i=127; i>0; i--) // sensor is inverted 180 deg because of lens so top is bottom and bottom is top so do this backwards or turn chip around on pcb!
{
CLK(ON);
DelayMicroseconds(1);
SI(OFF);
d = AnalogRead(ANA);
if(d>MAXBRIGHTNESS) RawData[i]=MAXBRIGHTNESS; else RawData[i]=d; // 255 is Max value we can get from TSL1401, but we have 10 bit ADC in PIC32 so we need to clip
if(SampleCount<MAXSAMPLES && SampleCount>2) AccumulatedData[i] = AccumulatedData[i] + RawData[i]; // accumulate total light across width of bottle so we can get an avg
CLK(OFF);
DelayMicroseconds(1);
}
if(SampleCount<MAXSAMPLES) SampleCount++; // increment how many samples in accumulator
CLK(ON);
DelayMicroseconds(1);
CLK(OFF);
if(SampleRate - 1500 > exposure)
DelayMicroseconds(SampleRate - 1500 - exposure); // hard-code delay so we get correct sample rate
}
int main(void) {
InitPeripherals();
mRedOFF; mGreenOFF; mBlueOFF;
// startup blink
const float kShortDelay = 0.1;
const float kLongDelay = 0.3;
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
mRedON; mGreenON; mBlueON;
DelaySeconds(kShortDelay);
mRedOFF; mGreenOFF; mBlueOFF;
DelaySeconds(kLongDelay);
UsbInterface usb = UsbInterface();
usb.Init();
PersistentMemory mem;
DelaySeconds(1.0f);
ZigbeeInterface zig = ZigbeeInterface();
DelaySeconds(0.05f);
zig.Init(mem);
error_reporting_com1 = &usb;
error_reporting_com2 = &zig;
Mpu60XXImu imu(mem);
GyroAccelDriftUkfIo est;
BatteryMonitor battery(mem);
battery_ptr = &battery;
MotorHal motor_hal(mem);
motor_hal_ptr = &motor_hal;
// Get_pos addition:
InitPositionSensor();
tim1_set_position_callback_2(get_position);
QuatPD pd(mem);
PulsingCoaxControllerQuat uav(mem, pd, imu, est, motor_hal);
CoaxOpenController open_controller(mem);
CoaxOpenAttitudeController open_attitude_controller(mem, est);
monitor = LoopMonitor(mem);
UavReporter reporter(mem, imu, est, motor_hal, battery, uav, open_attitude_controller, pd, monitor);
StateMachine state_machine;
mem.Freeze(); // freeze memory to make writes possible
imu.InitFromMemory();
//imu.DefaultAccelSensitivity(1); // overwrite scale for off-datasheet fix
imu.flip_z = 1; // flip z-axis direction for upside-down imu
#ifdef STREAM_IMU_RAW
imu_raw_msg_logger_init(imu);
#endif
monitor.InitFromMemory();
PlayTimebase();
imu.StartRead();
DelayMilliseconds(10);
tim1_init();
tim1_set_supply_volts(3.7f);
//////////////////////////////////////////////////////////////////////////////
// Main Loop
while(1) {
monitor.Profile(0);
////////////////////////////////////////////////////////////////////////////
// Get IMU Data and Start New Measurement
while(!imu.FinishRead()) {};
#ifdef STREAM_IMU_RAW
imu_raw_msg_logger_push();
#endif
DelayMicroseconds(50); // this seems to be critical (?!?)
imu.StartRead();
monitor.Profile(1);
////////////////////////////////////////////////////////////////////////////
// Update Estimator with Measurement
est.Update(imu.time, imu.w, imu.a);
////////////////////////////////////////////////////////////////////////////
// Control
// update control laws
uav.Update();
open_controller.Update();
open_attitude_controller.Update();
// map controllers to outputs based on state
enum control_state state = state_machine.get_state();
// in STOP state, send active kill messages to motors
if(state == kStop) {
mGreenOFF; mAmberON;
motor_hal.set_top_cmd_volts(0);
motor_hal.set_top_cmd_volts_pulse_amp(0);
motor_hal.set_top_cmd_pulse_phase(0);
motor_hal.set_bottom_cmd_volts(0);
}
// in STANDBY state, send no motor commands except on entry
else if(state == kStandby) {
mGreenOFF; mAmberOFF;
if(state_machine.get_standby_needs_init()) {
motor_hal.set_top_cmd_volts(0);
motor_hal.set_top_cmd_volts_pulse_amp(0);
motor_hal.set_top_cmd_pulse_phase(0);
motor_hal.set_bottom_cmd_volts(0);
state_machine.clear_standby_needs_init();
}
}
// in QUAT state, send motor commands according to quat control law
else if(state == kQuat) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(uav.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(uav.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(uav.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(uav.bottom_mean);
}
// in OPEN state, send motor command according to open motor control commands
else if(state == kOpen) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(open_controller.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(open_controller.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(open_controller.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(open_controller.bottom_mean);
}
// in OPEN ATTITUDE state, send motor command according to open motor control commands
else if(state == kOpenAttitude) {
mGreenON; mAmberOFF;
motor_hal.set_top_cmd_volts(open_attitude_controller.top_mean);
motor_hal.set_top_cmd_volts_pulse_amp(open_attitude_controller.top_pulse_amp);
motor_hal.set_top_cmd_pulse_phase(open_attitude_controller.top_pulse_phase);
motor_hal.set_bottom_cmd_volts(open_attitude_controller.bottom_mean);
}
monitor.Profile(2);
////////////////////////////////////////////////////////////////////////////
// Packet Communication
uint8_t is_data; // 1 iff data received
uint8_t *rx_data; // temporary pointer to received type+data bytes
uint8_t rx_length; // number of received type+data bytes
////////////////////////////////////////////////////////////////////////////
// USB Input
is_data = 0;
usb.GetBytes();
while(usb.PeekPacket(&rx_data, &rx_length)) {
zig.ReadMsg(usb, rx_data, rx_length);
imu.ReadMsg(usb, rx_data, rx_length);
est.ReadMsg(usb, rx_data, rx_length);
mem.ReadMsg(usb, rx_data, rx_length);
uav.ReadMsg(usb, rx_data, rx_length);
open_controller.ReadMsg( usb, rx_data, rx_length);
open_attitude_controller.ReadMsg( usb, rx_data, rx_length);
pd.ReadMsg( usb, rx_data, rx_length);
monitor.ReadMsg( usb, rx_data, rx_length);
motor_hal.ReadMsg(usb, rx_data, rx_length);
battery.ReadMsg( usb, rx_data, rx_length);
reporter.ReadMsg( usb, rx_data, rx_length);
state_machine.ReadMsg(usb, rx_data, rx_length);
usb.DropPacket();
is_data = 1;
} // while peek...
if(is_data) {
usb.SendNow();
}
////////////////////////////////////////////////////////////////////////////
// Radio Input
is_data = 0;
zig.GetBytes();
while(zig.PeekPacket(&rx_data, &rx_length)) {
#ifdef STREAM_IMU_RAW
if(rx_data[0] == kTypeQuatPilot || rx_data[0] == kTypeQuatFullObsPilot || rx_data[0] == kTypeOpenPilot || rx_data[0] == kTypeOpenAttitudePilot) {
imu_raw_msg_logger_send(zig);
}
#endif
zig.ReadMsg(zig, rx_data, rx_length);
imu.ReadMsg(zig, rx_data, rx_length);
est.ReadMsg(zig, rx_data, rx_length);
mem.ReadMsg(zig, rx_data, rx_length);
uav.ReadMsg(zig, rx_data, rx_length);
open_controller.ReadMsg( zig, rx_data, rx_length);
open_attitude_controller.ReadMsg( zig, rx_data, rx_length);
pd.ReadMsg( zig, rx_data, rx_length);
monitor.ReadMsg( zig, rx_data, rx_length);
motor_hal.ReadMsg(zig, rx_data, rx_length);
battery.ReadMsg( zig, rx_data, rx_length);
reporter.ReadMsg( zig, rx_data, rx_length);
state_machine.ReadMsg(zig, rx_data, rx_length);
zig.DropPacket();
is_data = 1;
} // while peek...
monitor.Profile(3);
if(is_data) {
zig.SendNow();
}
monitor.Profile(4);
monitor.Profile(5);
////////////////////////////////////////////////////////////////////////////
// Throttle main loop to main_freq
monitor.EndMainLoop();
monitor.Profile(6);
//monitor.SendProfile(usb);
//usb.SendNow();
} // while(1)
return(0);
}
int ZigBee_Init()
{
if ( ZigBee_CheckPartAndVers() == 0 )
{
#if defined(DEBUG)
printf("zigbee found!\r\n");
#endif
}
else
{
#if defined(DEBUG)
printf("zigbee NOT found!!\r\n");
#endif
return -1;
}
ZigBee_ConfigClock();
DelayMicroseconds(10);
if (ZigBee_ForceTrxOff() != 0)
return -1;
//enable Auto CRC, disable RX buffer empty IRQ
// ZigBee_WriteReg(REG_TRX_CTRL_1, 0x20);
// _delay_us(2);
//enable Auto CRC, disable RX buffer empty IRQ, Enable Amplifier+LEDs
ZigBee_WriteReg(REG_TRX_CTRL_1, 0xE0);
DelayMicroseconds(2);
// see TRX_CTRL_2 register descriptions in datasheet
ZigBee_WriteReg(REG_TRX_CTRL_2, 0b00001100); // modulation: OQPSK-SIN-250, fairly reliable within Modlab
// ZigBee_WriteReg(REG_TRX_CTRL_2, 0b00001101); // modulation: OQPSK-SIN-500, fairly reliable within Modlab
// ZigBee_WriteReg(REG_TRX_CTRL_2, 0b00101110); // modulation: OQPSK-SIN-1000-SCR-ON, high data loss at any range
DelayMicroseconds(2);
#ifndef AMPLIFIED_OUTPUT
ZigBee_WriteReg(REG_PHY_TX_PWR, 0xE1); //output power setting to 10dBm
#else
ZigBee_WriteReg(REG_PHY_TX_PWR, 0x84); //output power setting to 5dBm (Amplified)
mOrangeOFF;
#endif
if (ZigBee_SetModeRx() == 0)
{
#if defined(DEBUG)
printf("zigbee initialization complete.\r\n");
#endif
}
else
{
#if defined(DEBUG)
printf("zigbee initialization FAILED.\r\n");
#endif
/* while(1)
{
uint8_t zigbee_ret = ZigBee_ReadState();
switch(zigbee_ret)
{
case STATUS_P_ON:
mRedTOGGLE; break;
case STATUS_BUSY_RX:
mWhiteTOGGLE; break;
case STATUS_BUSY_TX:
mGreenTOGGLE; break;
//case STATUS_RX_ON:
//mAmberTOGGLE; break;
case STATUS_TRX_OFF:
mRedTOGGLE; mWhiteTOGGLE; break;
case STATUS_PLL_ON:
mRedTOGGLE; mGreenTOGGLE; break;
case 0xF:
mRedTOGGLE; mAmberTOGGLE; break;
case 0x11:
mRedTOGGLE; mWhiteTOGGLE; mGreenTOGGLE; break;
case 0x12:
mRedTOGGLE; mWhiteTOGGLE; mAmberTOGGLE; break;
case 0x16:
mRedTOGGLE; mWhiteTOGGLE; mAmberTOGGLE; mGreenTOGGLE; break;
case 0x19:
mWhiteTOGGLE; mGreenTOGGLE; break;
case 0x1C:
mWhiteTOGGLE; mAmberTOGGLE; break;
case 0x1D:
mWhiteTOGGLE; mGreenTOGGLE; mAmberTOGGLE; break;
case 0x1E:
mGreenTOGGLE; mAmberTOGGLE; break;
case 0x1F:
mAmberTOGGLE; break;
int irq_ret = ZigBee_ReadIrq();
if((irq_ret & 0x40) != 0)
{
// Frame Buffer access violation
mRedON;
mWhiteON;
}
else if((irq_ret & 0x02) != 0)
{
// PLL unlock
mRedON;
mGreenON;
}
else if((irq_ret & 0x01) != 0)
{
// PLL lock
mGreenON;
mWhiteON;
}
}
DelayMilliseconds(500);
} */
return -1;
}
return 0;
}
void DoSerial() // Serial Processing of any input data
{
char c;
uint8 i;
if(Serial2Available())
{
SerialConnected=1; // Client has connected so we can now send stuff.. (Set until POR as we dont have the equivelent for a disconnect)
c = Serial2Read();
if(c==3) // 3. Raw image test (focus setup etc)
{
while(c!=1 && c!=9)
{
OC_COUNT(ON);
AquireImage();
Serial2Write(3);
Serial2Write(99);
for(i=0; i<128; i++)
Serial2Write(RawData[i]);
Serial2Write(99);
DelayMicroseconds(100000);
if(Serial2Available()) c = Serial2Read();
}
}
if(c==4) // 4. end processing
{
PowerSaveSleep(); // END!
while(1);
}
if(c==5) // 5. Send fixed (NVM) settings
{
Serial2Write(5);
Serial2Write(99);
Serial2Write(Tmin);
Serial2Write(algorithm);
Serial2Write(cap);
Serial2Write(neck);
Serial2Write(SampleRate>>8);
Serial2Write(SampleRate);
Serial2Write(RejOnTime>>8);
Serial2Write(RejOnTime);
Serial2Write(tollerance);
}
if(c==6) SendCounters();// 6. Send current settings
if(c==7) // 7. Receive new settings temporary until reset or permanent if written to NVM
{
Tmin=Serial2Read();
algorithm=Serial2Read();
cap=Serial2Read();
neck=Serial2Read();
SampleRate=Serial2Read()<<8 + Serial2Read();
RejOnTime=Serial2Read()<<8 + Serial2Read();
tollerance=Serial2Read();
NVMChanged=1;
Serial2Write(7);
}
if(c==8 && NVMChanged) // 8. Save to NVM
{
if( (void*)(NVMPtr+8) >= NVM_END) // used all the flash page so we have to erase whole page and start again (will never happen seriously!)
{
NVMErasePage(NVM_START);
NVMPtr = NVM_START;
}
// Each variable is stored as a 32 bit integer which is the minimum size programable to flash NVMPtr will always be set at next free FLASH slot after POR or a write
NVMWriteWord( (void*) (NVMPtr++) , Tmin );
NVMWriteWord( (void*) (NVMPtr++) , algorithm );
NVMWriteWord( (void*) (NVMPtr++) , cap );
NVMWriteWord( (void*) (NVMPtr++) , neck );
NVMWriteWord( (void*) (NVMPtr++) , SampleRate );
NVMWriteWord( (void*) (NVMPtr++) , RejOnTime);
NVMWriteWord( (void*) (NVMPtr++) , tollerance);
NVMChanged = 0; // All good so unflag change
Serial2Write(8);
}
if(c==9) SoftReset(); // 9. RESET Sensor!
}
