void loop(void) {
if((millis()-timer)>=20) // Main loop runs at 50Hz
{
if(loop_counter++ > 50) {
loop_counter = 0;
//Serial.println(millis() - timer);
}
updateTime();
update_messages();
update_motors();
update_inertial_sensors();
update_control();
}
}
int main() {
//host.baud(115200);
xbee.baud(115200);
Ticker heartbeat_tkr;
heartbeat_tkr.attach_us(&beat, HEARTBEAT_UPDATE_RATE * 1000);
autopilotRemote.rise(&autopilot_ISR);
//i2c.frequency(400);
// Initialise PIDs
speedOverGroundPid.setInputLimits(0.0, SPEED_IN_KNOTS_LIMIT); // Assuming speed is in knots -- check this!
speedOverGroundPid.setOutputLimits(1, THROTTLE_LIMIT);
speedOverGroundPid.setMode(AUTO_MODE);
headingPid.setInputLimits(-180,180);
headingPid.setOutputLimits(0.0, THROTTLE_LIMIT);
headingPid.setMode(AUTO_MODE);
speedOverGroundPid.setSetPoint(2.5);
headingPid.setSetPoint(0);
DEBUG_OUTPUT(" _ _ _ _ \r\n"
" | | | | | | | | \r\n"
" ___| |__ ___ __| | | |__ ___ __ _| |_ \r\n"
"/ __| '_ \\ / _ \\/ _` | | '_ \\ / _ \\ / _` | __|\r\n"
"\\__ \\ | | | __/ (_| | | |_) | (_) | (_| | |_ \r\n"
"|___/_| |_|\\___|\\__,_| |_.__/ \\___/ \\__,_|\\__|\r\n"
" ______ \r\n"
" |______|\r\n"
"\r\n\n");
NMEA::init();
compass.init();
// Parkwood: 51.298997, 1.056683
// Chestfield: 51.349215, 1.066184
// Initialise Motors (Arming Sequence) -- If a value is not sent periodicially, then the motors WILL disarm!
leftMotor.set(0);
rightMotor.set(0);
wait(1);
setup_waypoints();
// Initialise Scheduler
Ticker gpsTelemetryUpdateTkr;
Ticker systemTelemetryUpdateTkr;
Ticker debugTelemetryUpdateTkr;
Ticker trackandSpeedUpdateTkr;
Ticker motorUpdateTkr;
gpsTelemetryUpdateTkr.attach_us(&gps_telemetry_update_ISR, GPS_TELEMETRY_UPDATE_RATE * 1000);
systemTelemetryUpdateTkr.attach_us(&system_telemetry_update_ISR, SYSTEM_TELEMETRY_UPDATE_RATE * 1000);
debugTelemetryUpdateTkr.attach_us(&debug_telemetry_update_ISR, DEBUG_TELEMETRY_UPDATE_RATE * 1000);
trackandSpeedUpdateTkr.attach_us(&track_and_speed_update_ISR, TRACK_UPDATE_RATE * 1000);
motorUpdateTkr.attach_us(&motor_update_ISR, MOTOR_UPDATE_RATE * 1000);
while(1) {
if(updateTrackAndSpeed && autopilotEngaged){
update_speed_and_heading();
updateTrackAndSpeed = false;
}
if(updateMotor && autopilotEngaged){
update_motors();
updateMotor = false;
}
if(updateGPSTelemetry){
gps_satellite_telemetry();
updateGPSTelemetry = false;
}
if(updateSystemTelemetry){
send_system_telemetry();
updateSystemTelemetry = false;
}
if(updateDebugTelemetry){
send_debug_telemetry();
updateDebugTelemetry = false;
}
}
}
// stability_test
void stability_test()
{
int16_t roll_in, pitch_in, yaw_in, throttle_in, avg_out;
int16_t testing_array[][4] = {
// roll, pitch, yaw, throttle
{ 0, 0, 0, 0},
{ 0, 0, 0, 100},
{ 0, 0, 0, 200},
{ 0, 0, 0, 300},
{ 4500, 0, 0, 300},
{ -4500, 0, 0, 300},
{ 0, 4500, 0, 300},
{ 0, -4500, 0, 300},
{ 0, 0, 4500, 300},
{ 0, 0, -4500, 300},
{ 0, 0, 0, 400},
{ 0, 0, 0, 500},
{ 0, 0, 0, 600},
{ 0, 0, 0, 700},
{ 0, 0, 0, 800},
{ 0, 0, 0, 900},
{ 0, 0, 0, 1000},
{ 4500, 0, 0, 1000},
{ -4500, 0, 0, 1000},
{ 0, 4500, 0, 1000},
{ 0, -4500, 0, 1000},
{ 0, 0, 4500, 1000},
{ 0, 0, -4500, 1000},
{5000, 1000, 0, 1000},
{5000, 2000, 0, 1000},
{5000, 3000, 0, 1000},
{5000, 4000, 0, 1000},
{5000, 5000, 0, 1000},
{5000, 0, 1000, 1000},
{5000, 0, 2000, 1000},
{5000, 0, 3000, 1000},
{5000, 0, 4500, 1000}
};
uint32_t testing_array_rows = 32;
hal.console->printf("\nTesting stability patch\nThrottle Min:%d Max:%d\n",(int)rc3.radio_min,(int)rc3.radio_max);
// arm motors
motors.armed(true);
motors.set_interlock(true);
motors.set_stabilizing(true);
// run stability test
for (uint16_t i=0; i<testing_array_rows; i++) {
roll_in = testing_array[i][0];
pitch_in = testing_array[i][1];
yaw_in = testing_array[i][2];
throttle_in = testing_array[i][3];
motors.set_pitch(roll_in);
motors.set_roll(pitch_in);
motors.set_yaw(yaw_in);
motors.set_throttle(throttle_in);
update_motors();
// calc average output
avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4);
// display input and output
hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t AvgOut:%5d\n",
(int)roll_in,
(int)pitch_in,
(int)yaw_in,
(int)throttle_in,
(int)hal.rcout->read(0),
(int)hal.rcout->read(1),
(int)hal.rcout->read(2),
(int)hal.rcout->read(3),
(int)avg_out);
}
// set all inputs to motor library to zero and disarm motors
motors.set_pitch(0);
motors.set_roll(0);
motors.set_yaw(0);
motors.set_throttle(0);
motors.armed(false);
hal.console->println("finished test.");
}
