Related
I'm in the process of working on a kernel program to handle printing capabilities of input for a custom OS. I'm following Poncho's 2nd YouTube Video series found here, I'm currently on Video 4 in the series where he starts to add numerical types as inputs to the renderer's print function. Now, my code isn't exactly like his as I made some modifications.
-Note- This won't compile directly as there is no main function. _start is being called or invoked by a bootloader that isn't shown here, I will however, add it to the bottom of this question.
When I use the class's print function like this within my kernel:
#include "BasicRenderer.h"
extern "C" void _start(Framebuffer* framebuffer, PSF1_FONT** fonts) {
BasicRenderer = renderer(framebuffer, fonts);
renderer.Print("This is some text");
renderer.Print('\n');
renderer.Print(uint64_t(123456789));
renderer.Print('\n');
renderer.Print(int64_t(-123456789));
return;
}
And I run the kernel in emu. I'm getting the following output displayed:
This is some text
123456789
-123456789
The above is correct, however, when I try to incorporate the ability to parse a newline set of characters being either \n or \0 within of a const char* that acts as a string as seen in the following example:
#include "BasicRenderer.h"
extern "C" void _start(Framebuffer* framebuffer, PSF1_FONT** fonts) {
BasicRenderer = renderer(framebuffer, fonts);
renderer.Print("This is some text\n");
renderer.Print(uint64_t(123456789));
renderer.Print('\n');
renderer.Print(int64_t(-123456789));
return;
}
And now the displayed output is:
This is some text
123456789
-123456789
Here, the output in the second line has a space preceding the numerical value to be displayed after the call to Print() that has a \n within its string. I'm not sure what is causing this in my code. Does it have to do with the while condition or how I'm incrementing and indexing into the character string within BasicRenderer::Print(const char* str)? Or is it coming from BasicRender::PutChar(char c)? Or is it within one of the to_string() functions?
Here is the relevant implementation code...
BasicRenderer.cpp
#include "BasicRenderer.h"
void BasicRenderer::Print(const char* str) {
char* chr = (char*)str;
while(*chr != 0) {
if ( (*chr == '\\') && ((*chr+1 == 'n') || (*chr+1 == '0')) ) {
PutChar('\n');
chr++;
chr++;
} else {
PutChar(*chr);
cursor_position_.x += 8;
if (cursor_position_.x + 8 > framebuffer_->Width) {
cursor_position_.x = 0;
cursor_position_.y += 16;
}
chr++;
}
}
}
void BasicRenderer::Print(uint64_t val) {
const char* str = to_string(val);
Print(str);
}
void BasicRenderer::Print(int64_t val) {
const char* str = to_string(val);
Print(str);
}
void BasicRenderer::PutChar(char c) {
if (c == '\n' || c == '\0') {
cursor_position_.x = 0;
cursor_position_.y += 16;
} else {
unsigned int* pixPtr = (unsigned int*)framebuffer_->BaseAddress;
char* fontPtr = (char*)selected_font_->glyphBuffer + (c * selected_font_->psf1_Header->charsize);
for (unsigned long y = cursor_position_.y; y < cursor_position_.y + 16; y++) {
for (unsigned long x = cursor_position_.x; x < cursor_position_.x + 8; x++) {
if ((*fontPtr & (0b10000000 >> (x - cursor_position_.x))) > 0) {
*(unsigned int*)(pixPtr + x + (y * framebuffer_->PixelsPerScanLine)) = font_color_;
}
}
fontPtr++;
}
}
}
cstr.cpp
#include "cstr.h"
const char* to_string(uint64_t value) {
static char output_uint_buffer[128];
uint8_t size = 0;
uint64_t sizeTest = value;
while (sizeTest / 10 > 0) {
sizeTest /= 10;
size++;
}
uint8_t idx = 0;
while (value / 10 > 0) {
uint8_t remainder = value % 10;
value /= 10;
output_uint_buffer[size - idx] = remainder + '0';
idx++;
}
uint8_t remainder = value % 10;
output_uint_buffer[size-idx] = remainder + '0';
output_uint_buffer[size + 1] = 0;
return output_uint_buffer;
}
const char* to_string(int64_t value) {
static char output_int_buffer[128];
uint8_t isNegative = 0;
if (value < 0) {
isNegative = 1;
value *= -1;
output_int_buffer[0] = '-';
}
uint8_t size = 0;
uint64_t sizeTest = value;
while (sizeTest / 10 > 0) {
sizeTest /= 10;
size++;
}
uint8_t idx = 0;
while (value / 10 > 0) {
uint8_t remainder = value % 10;
value /= 10;
output_int_buffer[isNegative + size - idx] = remainder + '0';
idx++;
}
uint8_t remainder = value % 10;
output_int_buffer[isNegative + size - idx] = remainder + '0';
output_int_buffer[isNegative + size + 1] = 0;
return output_int_buffer;
}
And here is the rest of the declarations...
BasicRender.h
#pragma once
#include "cstr.h"
#include "math.h"
#include "framebuffer.h"
#include "SimpleFonts.h"
class BasicRenderer {
public:
BasicRenderer(Framebuffer* framebuffer, PSF1_FONT** fonts) :
framebuffer_{framebuffer},
fonts_{fonts},
cursor_position_({0,0}),
selected_font_{fonts_[0]},
font_color_{0xFFFFFFFF}
{}
void Print(const char* str);
void Print(char c) { PutChar(c); }
void Print(uint64_t val);
void Print(int64_t val);
private:
void PutChar(char c);
Framebuffer* framebuffer_;
Point cursor_position_;
PSF1_FONT** fonts_;
PSF1_FONT* selected_font_;
unsigned int font_color_;
};
cstr.h
#pragma once
#include <stdint.h>
const char* to_string(uint64_t value);
const char* to_string(int64_t value);
math.h
#pragma once
struct Point {
unsigned int x;
unsigned int y;
};
Framebuffer.h
#pragma once
#include <stddef.h>
struct Framebuffer {
void* BaseAddress;
size_t BufferSize;
unsigned int Width;
unsigned int Height;
unsigned int PixelsPerScanLine;
};
SimpleFonts.h
#pragma once
struct PSF1_HEADER {
unsigned char magic[2];
unsigned char mode;
unsigned char charsize;
};
struct PSF1_FONT {
PSF1_HEADER* psf1_Header;
void* glyphBuffer;
};
Here is the bootloader application that invokes the above kernel.
main.c
#include <efi.h>
#include <efilib.h>
#include <elf.h>
#define PSF1_MAGIC0 0x36
#define PSF1_MAGIC1 0x04
typedef unsigned long long size_t;
typedef struct {
unsigned char magic[2];
unsigned char mode;
unsigned char charsize;
} PSF1_HEADER;
typedef struct {
PSF1_HEADER* psf1_Header;
void* glyphBuffer;
} PSF1_FONT;
typedef struct {
void* BaseAddress;
size_t BufferSize;
unsigned int Width;
unsigned int Height;
unsigned int PixelsPerScanLine;
} Framebuffer; Framebuffer framebuffer;
Framebuffer* InitializeGOP() {
EFI_GUID gopGuid = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID;
EFI_GRAPHICS_OUTPUT_PROTOCOL* gop;
EFI_STATUS status;
status = uefi_call_wrapper(BS->LocateProtocol, 3, &gopGuid, NULL, (void**)&gop);
if (EFI_ERROR(status)) {
Print(L"Unable to locate GOP\n\r");
return NULL;
} else {
Print(L"GOP located\n\r");
}
framebuffer.BaseAddress = (void*)gop->Mode->FrameBufferBase;
framebuffer.BufferSize = gop->Mode->FrameBufferSize;
framebuffer.Width = gop->Mode->Info->HorizontalResolution;
framebuffer.Height = gop->Mode->Info->VerticalResolution;
framebuffer.PixelsPerScanLine = gop->Mode->Info->PixelsPerScanLine;
return &framebuffer;
}
EFI_FILE* LoadFile(EFI_FILE* Directory, CHAR16* Path, EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE* SystemTable) {
EFI_FILE* LoadedFile;
EFI_LOADED_IMAGE_PROTOCOL* LoadedImage;
SystemTable->BootServices->HandleProtocol(ImageHandle, &gEfiLoadedImageProtocolGuid, (void**)&LoadedImage);
EFI_SIMPLE_FILE_SYSTEM_PROTOCOL* FileSystem;
SystemTable->BootServices->HandleProtocol(LoadedImage->DeviceHandle, &gEfiSimpleFileSystemProtocolGuid, (void**)&FileSystem);
if (Directory == NULL) {
FileSystem->OpenVolume(FileSystem, &Directory);
}
EFI_STATUS s = Directory->Open(Directory, &LoadedFile, Path, EFI_FILE_MODE_READ, EFI_FILE_READ_ONLY);
if (s != EFI_SUCCESS) {
return NULL;
}
return LoadedFile;
}
PSF1_FONT* LoadPSF1Font(EFI_FILE* Directory, CHAR16* Path, EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE* SystemTable) {
EFI_FILE* font = LoadFile(Directory, Path, ImageHandle, SystemTable);
if (font == NULL) return NULL;
PSF1_HEADER* fontHeader;
SystemTable->BootServices->AllocatePool(EfiLoaderData, sizeof(PSF1_HEADER), (void**)&fontHeader);
UINTN size = sizeof(PSF1_HEADER);
font->Read(font, &size, fontHeader);
if (fontHeader->magic[0] != PSF1_MAGIC0 || fontHeader->magic[1] != PSF1_MAGIC1) return NULL;
UINTN glyphBufferSize = fontHeader->charsize * 256;
if (fontHeader->mode == 1) { // 512 glyph mode
glyphBufferSize *= 2;
}
void* glyphBuffer;
font->SetPosition(font, sizeof(PSF1_HEADER));
SystemTable->BootServices->AllocatePool(EfiLoaderData, glyphBufferSize, (void**)&glyphBuffer);
font->Read(font, &glyphBufferSize, glyphBuffer);
PSF1_FONT* finishedFont;
SystemTable->BootServices->AllocatePool(EfiLoaderData, sizeof(PSF1_FONT), (void**)&finishedFont);
finishedFont->psf1_Header = fontHeader;
finishedFont->glyphBuffer = glyphBuffer;
return finishedFont;
}
int memcmp(const void* aptr, const void* bptr, size_t n) {
const unsigned char* a = aptr, *b = bptr;
for (size_t i = 0; i < n; i++) {
if (a[i] < b[i]) return -1;
else if(a[i] > b[i]) return 1;
}
return 0;
}
EFI_STATUS efi_main (EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable) {
InitializeLib(ImageHandle, SystemTable);
Print(L"Hello World!\n\r");
EFI_FILE* Kernel = LoadFile(NULL, L"kernel.elf", ImageHandle, SystemTable);
if ( Kernel == NULL) {
Print(L"Could not load kernel \n\r");
} else {
Print(L"Kernel Loaded Successfully \n\r");
}
Elf64_Ehdr header;
{
UINTN FileInfoSize;
EFI_FILE_INFO* FileInfo;
Kernel->GetInfo(Kernel, &gEfiFileInfoGuid, &FileInfoSize, NULL);
SystemTable->BootServices->AllocatePool(EfiLoaderData, FileInfoSize, (void**)&FileInfo);
Kernel->GetInfo(Kernel, &gEfiFileInfoGuid, &FileInfoSize, (void**)&FileInfo);
UINTN size = sizeof(header);
Kernel->Read(Kernel, &size, &header);
}
if (
memcmp(&header.e_ident[EI_MAG0], ELFMAG, SELFMAG) != 0 ||
header.e_ident[EI_CLASS] != ELFCLASS64 ||
header.e_ident[EI_DATA] != ELFDATA2LSB ||
header.e_type != ET_EXEC ||
header.e_machine != EM_X86_64 ||
header.e_version != EV_CURRENT
) {
Print(L"kernel format is bad\r\n");
} else {
Print(L"kernel header successfully verified\r\n");
}
Elf64_Phdr* phdrs;
{
Kernel->SetPosition(Kernel, header.e_phoff);
UINTN size = header.e_phnum * header.e_phentsize;
SystemTable->BootServices->AllocatePool(EfiLoaderData, size, (void**)&phdrs);
Kernel->Read(Kernel, &size, phdrs);
}
for (
Elf64_Phdr* phdr = phdrs;
(char*)phdr < (char*)phdrs + header.e_phnum * header.e_phentsize;
phdr = (Elf64_Phdr*)((char*)phdr + header.e_phentsize)
) {
switch(phdr->p_type) {
case PT_LOAD: {
int pages = (phdr->p_memsz + 0x1000 - 1) / 0x1000;
Elf64_Addr segment = phdr->p_paddr;
SystemTable->BootServices->AllocatePages(AllocateAddress, EfiLoaderData, pages, &segment);
Kernel->SetPosition(Kernel, phdr->p_offset);
UINTN size = phdr->p_filesz;
Kernel->Read(Kernel, &size, (void*)segment);
break;
}
}
}
Print(L"Kernel Loaded\n\r");
void (*KernelStart)(Framebuffer*, PSF1_FONT**) = ((__attribute__((sysv_abi)) void(*)(Framebuffer*, PSF1_FONT**) ) header.e_entry);
PSF1_FONT* newFont = LoadPSF1Font(NULL, L"zap-light16.psf", ImageHandle, SystemTable);
if (newFont == NULL) {
Print(L"Font is not valid or is not found\n\r");
} else {
Print(L"Font found, char size = %d\n\r", newFont->psf1_Header->charsize);
}
PSF1_FONT* newFontExt = LoadPSF1Font(NULL, L"zap-ext-light16.psf", ImageHandle, SystemTable);
if (newFont == NULL) {
Print(L"Font is not valid or is not found\n\r");
} else {
Print(L"Font found, char size = %d\n\r", newFont->psf1_Header->charsize);
}
PSF1_FONT* fonts[] = {newFont, newFontExt};
Framebuffer* newBuffer = InitializeGOP();
Print(L"Base: 0x%x\n\rSize: 0x%x\n\rWidth: %d\n\rHeight: %d\n\rPixelsPerScanline: %d\n\r",
newBuffer->BaseAddress,
newBuffer->BufferSize,
newBuffer->Width,
newBuffer->Height,
newBuffer->PixelsPerScanLine);
KernelStart(newBuffer, fonts);
return EFI_SUCCESS; // Exit the UEFI application
}
The problem is here:
if ( (*chr == '\\') && ((*chr+1 == 'n') || (*chr+1 == '0')) ) {
PutChar('\n');
chr++;
chr++;
}
...
You should not be parsing out \n since this will be present in the string as a linefeed character. What you want instead is:
if (*chr == '\n') {
PutChar('\n');
chr++;
}
...
I am new to Arduino and I'm trying to make a program that receives IR codes from a TV remote, uses them as a 4 number pass code lighting up a LED as you press each button. And then comparing the code to a hard-coded one. In this case 1234.
I made a function to verify that the value entered is equal to the pass. If so, light up a green LED and else, light up a red one.
However, even if I input the correct code, only the red led lights up.
Here is my whole code as I'm not sure which part of it is the one causing problems:
const int pass[4] = {1, 2, 3, 4};
int value[4] = {};
int digitNum = 0;
int input;
void loop()
{
value[digitNum] = input; //where input is a number between 0 and 9
digitNum++;
if(digitNum == 1){
lightFirstLed();
}
else if(digitNum == 2){
lightSecondLed();
}
else if(digitNum == 3){
lightThirdLed();
}
else if(digitNum == 4){
lightFourthLed();
verify();
}
}
void verify()
{
bool falseCharacter = false;
for(int i = 0; i <= 4; i++){
if(value[i] != pass[i]){
falseCharacter = true;
}
}
if(!falseCharacter){
lightGreenLed();
}
else{
lightRedLed();
}
}
Functions in the form of light*Led actually do what they're supposed to do.
I tried changing the verify function around, that ended up making the green LED the one that always shone. I've been doing this for hours and I'm starting to feel disparate.
I would really appreciate any help. And please tell me if anything I'm doing does not comply with best practices even if it's out of the scope of this question.
For full code and design, here's a link to autodesk's simulator: https://www.tinkercad.com/things/0keqmlhVqNp-mighty-leelo/editel?tenant=circuits?sharecode=vVUD2_4774Lj4PYXh6doFcOqWUMY2URIfW8VXGxutRE=
EDIT: Now reset doesn't work
Your for loop in verify is accessing outside the array:
const int pass[4] = {1, 2, 3, 4};
int value[4] = {};
for(int i = 0; i <= 4; i++){
if(value[i] != pass[i]){
falseCharacter = true;
}
}
Change i <= 4 to i < 4. Also, when falseCharacter is set to true, break from the loop:
for(int i = 0; i < 4; i++)
{
if(value[i] != pass[i])
{
falseCharacter = true;
break;
}
}
Update
You need an else statement in loop:
void loop(void)
{
if(irrecv.decode(&results))
{
if (results.value == powBtn)
{
reset();
}
else if (results.value == zeroBtn)
{
input = 0;
}
else if (results.value == oneBtn)
{
input = 1;
}
else if (results.value == twoBtn)
{
input = 2;
}
else if (results.value == threeBtn)
{
input = 3;
}
else if (results.value == fourBtn)
{
input = 4;
}
else if (results.value == fiveBtn)
{
input = 5;
}
else if (results.value == sixBtn)
{
input = 6;
}
else if (results.value == sevenBtn)
{
input = 7;
}
else if (results.value == eightBtn)
{
input = 8;
}
else if (results.value == nineBtn)
{
input = 9;
}
else
{
return; /*** !!! Unrecognized Value !!! ***/
}
value[digitNum] = input;
digitNum++;
if(digitNum == 1)
{
digitalWrite(LED1, HIGH);
}
else if(digitNum == 2)
{
digitalWrite(LED2, HIGH);
}
else if(digitNum == 3)
{
digitalWrite(LED3, HIGH);
}
else if(digitNum == 4)
{
digitalWrite(LED4, HIGH);
verify();
}
else
{
if (results.value == powBtn)
{
reset();
}
}
// Receive the next value
irrecv.resume();
}
}
For school I am making a clapping sensor with my arduino nano. I have found some code to detect if there are 2 claps (link). But now I want to modify the code so it can distinguish if I clapped 1,2 or 3 times. I have now changed the source to detect 1 or 2 claps. But now if I clap twice, always a one clap is detected before it sees the 2 claps. And i have totally no idea how to detect 3 claps. Can someone please help me with this problem?
Code:
#define signalToRelayPin 12
#define sensorPin 7
int lastSoundValue;
int soundValue;
long lastNoiseTime = 0;
long currentNoiseTime = 0;
long lastLightChange = 0;
int relayStatus = HIGH;
void setup() {
pinMode(sensorPin, INPUT);
pinMode(signalToRelayPin, OUTPUT);
Serial.begin(115200);
}
struct DataBlockStruct meting1,meting2;
void loop() {
soundValue = digitalRead(sensorPin);
currentNoiseTime = millis();
if (soundValue == 1) { // if there is currently a noise
if (
(currentNoiseTime > lastNoiseTime + 200) && // to debounce a sound occurring in more than a loop cycle as a single noise
(lastSoundValue == 0) && // if it was silent before
(currentNoiseTime < lastNoiseTime + 800) && // if current clap is less than 0.8 seconds after the first clap
(currentNoiseTime > lastLightChange + 1000) // to avoid taking a third clap as part of a pattern
) {
relayStatus = !relayStatus;
Serial.println("2 X CLAP");
} else {
Serial.println("1 X CLAP");
}
lastNoiseTime = currentNoiseTime;
}
lastSoundValue = soundValue;
}
Try this snippet:
#define signalToRelayPin 12
#define sensorPin 7
int lastSoundValue;
int soundValue;
long lastNoiseTime = 0;
long currentNoiseTime = 0;
long lastLightChange = 0;
int relayStatus = HIGH;
int clap_interval = 500;
int claps = 0;
void setup() {
pinMode(sensorPin, INPUT);
pinMode(signalToRelayPin, OUTPUT);
Serial.begin(115200);
}
struct DataBlockStruct meting1,meting2;
void loop() {
soundValue = digitalRead(sensorPin);
currentNoiseTime = millis();
if (soundValue == 1 && lastSoundValue == 0)
{
if (claps == 0) // allow first to register without much condition
{
claps = 1;
lastNoiseTime = currentNoiseTime;
}
else
{
if (currentNoiseTime > lastNoiseTime + clap_interval)
{
claps++;
lastNoiseTime = currentNoiseTime;
relayStatus = !relayStatus;
}
}
}
else
{
if (currentNoiseTime > lastNoiseTime + 2 * clap_interval) // no claps for a longer duration time to print and/or reset clap
{
if (claps > 0)
{
Serial.print(claps);
Serial.println(" CLAPS");
claps = 0; ///reset
}
}
}
//lastSoundValue = soundValue;
delay(50); // delay polling
}
I have a matrix relay with a nanopi fire (like rasperry pi). I connected this relay to my device and want to change gpio value.(my device has multi gpio pin. first I should create gpio(echo 95 > /sys/class/gpio/export) and then change direction folder "out" (echo "out" > /sys/class/gpio/gpio95/direction) and with changing value the relay should turn on and off.)
this is my code:
bool GPIOClass::ChangeValue(QString IO, QString port)
{
QString gpio;
QString comand;
// comand=" echo ";
// comand.append(QIO);
// comand.append(" > ");
// gpio=config->GpioFromPort(port);
// if(gpio.length()==0)
// return false;
// //IO should be 1 or 0
QString src=IdenGPIOSrc(gpio);//src is /sys/class/gpio/gpio95
src.append("/value");
// comand.append(src);
// qDebug()<<port<<gpio<<"with src:"
// <<src <<"with command:"<<comand;
// system(comand.toStdString().c_str());
///===========================================================
QFile file(src);
if ( file.open(QIODevice::WriteOnly) )
{
//clear the content
// file.resize(0);
file.seek(0);
file.write(QByteArray::number(IO.toInt()),1);
}
if(GetValue(port)==IO.toInt())
{
return true;
}
else
return false;
}
the relay is not changed, but when I run the executable sample the relay is turn on and off:
int main(int argc, char ** argv)
{
int pin = GPIO_PIN(7);
int i, value, board;
int ret = -1;
if ((board = boardInit()) < 0) {
printf("Fail to init board\n");
return -1;
}
if (board == BOARD_NANOPI_T2)
pin = GPIO_PIN(15);
if (argc == 2)
pin = GPIO_PIN(atoi(argv[1]));
if ((ret = exportGPIOPin(pin)) == -1) {
printf("exportGPIOPin(%d) failed\n", pin);
}
if ((ret = setGPIODirection(pin, GPIO_OUT)) == -1) {
printf("setGPIODirection(%d) failed\n", pin);
}
for (i = 0; i < STATUS_CHANGE_TIMES; i++) {
if (i % 2) {
value = GPIO_HIGH;
} else {
value = GPIO_LOW;
}
if ((ret = setGPIOValue(pin, value)) > 0) {
printf("%d: GPIO_PIN(%d) value is %d\n", i+1, pin, value);
} else {
printf("setGPIOValue(%d) failed\n", pin);
}
sleep(1);
}
unexportGPIOPin(pin);
return 0;
}
in that code also the value in that address changed like my code (change value file to 1 and 0 ) and the relay is working. where is the diffrence between my code and that code? both of them change the value content.
where is my code problem?
I have an issue where my Arduino program hangs for no reason. I run my program, and at some undetermined point, the Serial Monitor stops printing output. Here's what I've tested so far:
In my "com.init_drone()" method, I've commented everything out except for the last line, which signals that the method returned. When I do this, my program hangs somewhere else but still it doesn't get to the infinite while loop.
I've been outputting memory usage, and I'm getting numbers no lower than 450 -- this tells me that I'm not using an absurd amount of memory.
I've tried removing the Timer1 instantiation, interrupt attach/detach, bu that has had no effect on the program.
My .ino file(and Command) is located here for anyone that wants a fuller picture and doesn't want to scroll through all this code I'm going to post below.
Here's my log output so far. Notice the truncation!:
AT&F
AT+NMAC=00:1d:c9:10:39:6f
AT+WM=0
AT+NDHCP=1
AT+WA=ardrone_279440
AT+NCUDP=192.168.1.1,5556
S0AT*CONFIG=1,"general:navdata_demo","TRUE"
EAT*CONFIG=1,"general:navdata_demo","TRUE"
638
S0AT*CONFIG=2,"control:altitude_max","2000"
EAT*CONFIG=2,"control:altitude_max","2000"
638
S0AT*CONFIG=3,"control:euler_angle_max","0.35"
EAT*CONFIG=3,"control:euler_angle_max","0.35"
586
S0AT*CONFIG=4,"control:outdoor","FALSE"
EAT*CONFIG=4,"control:outdoor","FALSE"
635
S0AT*CONFIG=5,"control:flight_without_shell","FALSE"
EAT*CONFIG=5,"control:flight_without_shell","FALSE"
574
S0AT*CTRL=6,4,0
EAT*CTRL=6,4,0
629
S0AT*CTRL=7,0,0
EAT*CTRL=7,0,0
629
S0AT*CTRL=8,4,0
EAT*CTRL=8,4,0
629
S0AT*COMWDG=9
EAT*COMWDG=9
629
S0AT*COMWDG=10
EAT*COMWDG=10
629
S0AT*COMWDG=11
EAT*COMWDG=11
629
S0AT*COMWDG=12
EAT*COMWDG=12
629
S0AT*COMWDG=13
EAT*COMWDG=13
629
S0AT*FTRIM=14
EAT*FTRIM=14
629
Here is my .ino file:
#include "Command.h"
#include "Streaming.h"
int debug = 1;
extern ring_buffer rx_buf;
extern resultint_ resultint;
Command com;
int sequenceNumber = 1;
String atcmd = "";
#include "TimerOne.h"
#define LEDpin 13
void setup()
{
PCsrl.begin(9600);
com.start_wifi_connection();
com.drone_is_init = com.init_drone();
Timer1.initialize(COMWDG_INTERVAL_USEC);
Timer1.attachInterrupt(watchdog_timer);
}
void watchdog_timer() {
com.sendwifi(com.makeComwdg());
}
void loop()
{
if (com.drone_is_init == 0) {
if (debug) {
// never use three ! together in arduino code
PCsrl.println("Drone wasn't initlized before loop() was called. Initalizing now.\r\n");
}
} else {
com.drone_takeoff();
com.drone_takeoff();
com.sendwifi(com.makePcmd(1,0,0,0,0));
com.sendwifi(com.makePcmd(1,0,0,0,0));
delay(5000);
com.moveForward(1);
com.moveRotate(180);
com.moveForward(1);
com.moveRotate(180);
delay(500);
com.drone_landing();
com.drone_landing();
delay(500);
//end of program
Timer1.detachInterrupt();
PCsrl.println("Program finished");
while (1){};
}
}
And my Command.cpp
#ifndef GAINSPAN
#define GAINSPAN
#include "Command.h"
extern int sequenceNumber;
extern int debug;
ring_buffer rx_buf= {{0}, 0, 0};
resultint_ resultint;
Command::Command()
{
at = "";
command = "";
s2ip_running = 0;
drone_is_init = 0;
drone_is_hover = 0;
emergency = 0;
}
void Command::sendwifi(String s) {
WIFIsrl.write(27); //esc
WIFIsrl.print("S0"); //choose connection CID 0
WIFIsrl.print(s);
WIFIsrl.write(27);
WIFIsrl.print("E");
if(debug) PCsrl.println(s);
WIFIsrl.println(memoryTest());
}
int Command::start_wifi_connection()
{
WIFIsrl.begin(9600);
WIFIsrl.println("");
WIFIsrl.println("AT&F");
//WIFIsrl.println("ATE0"); //turn off echo
WIFIsrl.print("AT+NMAC=00:1d:c9:10:39:6f\r"); //set MAC address
WIFIsrl.println("AT+WM=0");
WIFIsrl.println("AT+NDHCP=1");
/* drone's network profile, change if needed*/
WIFIsrl.println("AT+WA=ardrone_279440");
WIFIsrl.println("AT+NCUDP=192.168.1.1,5556");
readARsrl();
delay(3000); //need 3 seconds for connection to establish
return 0;
}
String Command::makeComwdg()
{
at = "AT*COMWDG=";
command = at + getSequenceNumber() + "\r\n";
return command;
}
void Command::sendComwdg_t(int msec)
{
for (int i = 0; i < msec; i+=20) {
sendwifi(makeComwdg());
delay(20);
}
}
void Command::sendFtrim()
{
at = "AT*FTRIM=";
command = at + getSequenceNumber() + "\r\n";
sendwifi(command);
}
void Command::sendConfig(String option, String value)
{
at = "AT*CONFIG=";
command = at + getSequenceNumber() + ",\"" + option + "\",\"" + value + "\"\r\n";
sendwifi(command);
}
void Command::sendRef(flying_status fs)
{
at = "AT*REF=";
if(fs == TAKEOFF){
command = at + getSequenceNumber() + ",290718208\r\n"; //takeoff
}
else if(fs == LANDING){
command = at + getSequenceNumber() + ",290717696\r\n"; //landing
} else if (fs == EMERGENCY_TOGGLE){
command = at + getSequenceNumber() + ",290717952\r\n"; //landing
}
// emergency -> 290717952
sendwifi(command);
}
void Command::send_control_commands(){
at = "AT*CTRL=";
sendwifi(at+getSequenceNumber()+",4,0\r\n");
sendwifi(at+getSequenceNumber()+",0,0\r\n");
sendwifi(at+getSequenceNumber()+",4,0\r\n");
}
void Command::drone_emergency_reset()
{
at = "AT*REF=";
command = at + getSequenceNumber() + ",290717952\r\n";
sendwifi(command);
}
/** Movement functions **/
int Command::moveForward(float distanceInMeters)
{
float i = 0;
String moveForward = makePcmd(1, 0, -.855, 0, 0);
delay(1000*distanceInMeters);
sendPcmd(moveForward);
return 1;
}
int Command::moveRotate(float yawInDegrees)
{
int i = 0;
while (i < yawInDegrees) {
String stayRotate = makePcmd(1, 0, 0, 0, 0.17);
sendPcmd(stayRotate);
delay(150);
i += 8;
}
return 1;
}
String Command::makePcmd(int enable, float roll, float pitch, float gaz, float yaw)
{
at = "AT*PCMD=";
command = at + getSequenceNumber() + "," + enable + "," + fl2int(roll) + "," + fl2int(pitch) + "," + fl2int(gaz) + "," + fl2int(yaw) + "\r";
return command;
}
void Command::sendPcmd(String command)
{
previousCommand = command;
sendwifi(command);
}
void Command::sendPcmd(int enable, float roll, float pitch, float gaz, float yaw)
{
at = "AT*PCMD=";
command = at + getSequenceNumber() + "," + enable + "," + fl2int(roll) + "," + fl2int(pitch) + "," + fl2int(gaz) + "," + fl2int(yaw) + "\r";
sendwifi(command);
}
String Command::makeAnim(anim_mayday_t anim, int time)
{
at = "AT*ANIM=";
command = at + getSequenceNumber() + "," + anim + "," + time + "\r\n";
return command;
}
void Command::doLEDAnim(int animseq, int duration)
{
PCsrl << "calling LEDAnim" << endl;
at = "AT*LED=";
command = at + getSequenceNumber() + "," + animseq + ",1073741824," + duration + "\r\n";
sendwifi(command);
}
int Command::start_s2ip()
{
char temp;
//delay(20000); //wait for drone to start
readARsrl();
if (debug) {
PCsrl << "trying to start s2ip" << endl;
}
ARsrl.print("\r\n");
delay(500);
ARsrl.print("\r\n");
delay(500);
ARsrl << "cd ~" << endl;
if (debug) {
readARsrl();
}
delay(500);
ARsrl << "cd data/video/apps/" << endl;
delay(500);
ARsrl << "./s2ip.arm" << endl;
while ((int) temp != 2) {
temp = ARsrl.read();
if (temp == 2) {
PCsrl << "s2ip is running" << endl;
ARsrl << "bullshit\r\n"; //to fix a delay bug
break;
}
//PCsrl << "s2ip not running" << endl;
}
if (debug) {
while (ARsrl.available()) {
PCsrl.write(ARsrl.read());
}
}
return 1;
}
void Command::quit_s2ip()
{
ARsrl.println("EXIT");
while (ARsrl.available()) {
PCsrl.write(ARsrl.read());
}
}
int Command::init_drone()
{
sendConfig("general:navdata_demo","TRUE");
sendConfig("control:altitude_max","2000");
sendConfig("control:euler_angle_max","0.35");
sendConfig("control:outdoor","FALSE");
sendConfig("control:flight_without_shell","FALSE");
send_control_commands();
sendComwdg_t(90);
sendFtrim();
drone_emergency_reset(); //clear emergency flag
return 1;
}
int Command::drone_takeoff()
{
sendRef(TAKEOFF);
int i = 0;
return 1;
}
int Command::drone_hover(int msec)
{
int i = 0;
while (i < msec) {
sendwifi(makePcmd(1, 0, 0, 0, 0));
delay(100);
i += 100;
}
return 1;
}
int Command::drone_landing()
{
sendRef(LANDING);
return 1;
}
int Command::drone_move_up(int centimeter)
{
int i = 0;
while (i < centimeter) {
ARsrl << makePcmd(1, 0, 0, 0.6, 0);
delay(100);
i += 10;
}
return 1;
}
int Command::drone_move_down(int centimeter)
{
int i = 0;
while (i < centimeter) {
sendwifi(makePcmd(1, 0, 0, -0.5, 0));
delay(100);
i += 10;
}
return 1;
}
long Command::fl2int(float value)
{
resultint.i = 0;
if (value < -1 || value > 1) {
resultint.f = 1;
} else {
resultint.f=value;
}
return resultint.i;
}
void Command::readARsrl()
{
while (ARsrl.available()) {
if (debug) {
PCsrl.write(ARsrl.read());
}
}
}
//Memory test code from : http://www.faludi.com/2007/04/18/arduino-available-memory-test/
int Command::memoryTest() {
int byteCounter = 0; // initialize a counter
byte *byteArray; // create a pointer to a byte array
// More on pointers here: http://en.wikipedia.org/wiki/Pointer#C_pointers
// use the malloc function to repeatedly attempt
// allocating a certain number of bytes to memory
// More on malloc here: http://en.wikipedia.org/wiki/Malloc
while ( (byteArray = (byte*) malloc (byteCounter * sizeof(byte))) != NULL ) {
byteCounter++; // if allocation was successful, then up the count for the next try
free(byteArray); // free memory after allocating it
}
free(byteArray); // also free memory after the function finishes
return byteCounter; // send back the highest number of bytes successfully allocated
}
int Command::getSequenceNumber(){
return sequenceNumber++;
}
// Volatile, since it is modified in an ISR.
volatile boolean inService = false;
void SrlRead()
{
if (inService) {
PCsrl.println("timer kicked too fast");
return;
}
interrupts();
inService = true;
while(ARsrl.available()) {
unsigned char k = ARsrl.read();
store_char(k, &rx_buf);
}
inService = false;
}
void read_rx_buf()
{
while (rx_buf.tail != rx_buf.head) {
if (debug) {
PCsrl.write(rx_buf.buffer[rx_buf.tail]);
}
rx_buf.tail = (unsigned int) (rx_buf.tail+ 1) % SERIAL_BUFFER_SIZE;
}
}
inline void store_char(unsigned char c, ring_buffer *buffer)
{
int i = (unsigned int)(buffer->head + 1) % SERIAL_BUFFER_SIZE;
// if we should be storing the received character into the location
// just before the tail (meaning that the head would advance to the
// current location of the tail), we're about to overflow the buffer
// and so we don't write the character or advance the head.
if (i != buffer->tail) {
buffer->buffer[buffer->head] = c;
buffer->head = i;
}
else {
Serial.println("ring buffer is too small");
}
}
#endif
I know it sounds weird but, sometimes this happens when arduino is not getting enough power supply. Try connecting the arduino to a power source different from USB.
As soon as I started to put things in prog memory my program started to clear up it's hiccups. It seems it was a memory issue.
I had the same problem, but the issue was with Timer1.initialize(). Try this:
com.drone_is_init = com.init_drone();
Serial.println("One");
Serial.println("Two");
Serial.println("Three");
Timer1.initialize(COMWDG_INTERVAL_USEC);
Timer1.attachInterrupt(watchdog_timer);
Open serial monitor and see... it will show until "Two", and then Arduino will hang.
The issue was I calling some functions of the LiquidCrystal_I2C library, that need interrupt routines. Check if your timer ISR is using some interrupts. If so, you should move this code to another place in your project.