Browse Source

Working Gyro Angle

master
Englebert 3 years ago
parent
commit
95ee61f22c
  1. 6
      README.md
  2. 373
      REF.md
  3. BIN
      configurator/gzip/index.html.gz
  4. 159
      configurator/src/index.html
  5. 2
      getFeeds.sh
  6. 3
      src/Motors.cpp
  7. 2
      src/Motors.h
  8. 170
      src/gyro.cpp
  9. 21
      src/gyro.h
  10. 18
      src/main.cpp
  11. 24
      src/main.h
  12. 101
      src/web.cpp

6
README.md

@ -311,6 +311,9 @@ REF:
- https://www.digikey.at/en/maker/projects/introduction-to-rtos-solution-to-part-12-multicore-systems/369936f5671d4207a2c954c0637e7d50
- BMP280 - https://community.blynk.cc/t/esp32-bmp280-st7735-mcp2301-blynk/38933
- SPIFFS - https://techtutorialsx.com/2019/03/04/esp32-arduino-serving-bootstrap/
- TeaPOT - https://www.hackster.io/Aritro/visualising-3d-motion-of-imu-sensor-3933b0?ref=user&ref_id=15798&offset=0
- Gyro - https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
- Gyro - https://electronoobs.com/eng_robotica_tut9_code5.php
### PROBLEM
@ -324,3 +327,6 @@ REF:
### DELET FILES FROM LITTEFS
curl -s http://192.168.1.1/erase?filename=index.html.gz

373
REF.md

@ -0,0 +1,373 @@
Arduino + MAX7456
https://www.hobbytronics.co.uk/download/MAX7456_OSDTEST.ino
/*
MAX7456 OSD Display Test Program for Arduino
Hobbytronics.co.uk
Nov-2013
Setup is for PAL but NTSC settings are shown in the program.
Displays text on the screen in to following format (30 chars x 16 lines)
The program is completely self contained and requires no library files
'------------------------------`
| |
| Hobbytronics.co.uk OSD |
| Test |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| #### 00:00:00 | <- Clock counts up from boot
| |
'------------------------------`
*/
// SPI pins
#define DATAOUT 11 //MOSI
#define DATAIN 12 //MISO
#define SPICLOCK 13 //sck
#define MAX7456SELECT 10 //ss
//MAX7456 opcodes
#define MAX_DMM_REG 0x04
#define MAX_DMAH_REG 0x05
#define MAX_DMAL_REG 0x06
#define MAX_DMDI_REG 0x07
#define MAX_VM0_REG 0x00
#define MAX_VM1_REG 0x01
#define MAX_CMAH_REG 0x09
#define MAX_CMM_REG 0x08
//MAX7456 commands
#define MAX_CLEAR_DISPLAY 0x04
#define MAX_CLEAR_DISPLAY_VERT 0x06
#define MAX_END_STRING 0xFF
// PAL
// all VM0_reg commands need bit 6 set
#define MAX_ENABLE_DISPLAY 0x48
#define MAX_ENABLE_DISPLAY_VERT 0x4C
#define MAX_RESET 0x42
#define MAX_DISABLE_DISPLAY 0x40
#define MAX_SCREEN_SIZE 480
#define MAX_SCREEN_WIDTH 30
#define MAX_SCREEN_ROWS 16
// NTSC
//#define MAX_ENABLE_DISPLAY 0x08
//#define MAX_ENABLE_DISPLAY_VERT 0x0C
//#define MAX_RESET 0x02
//#define MAX_DISABLE_DISPLAY 0x00
//#define MAX_SCREEN_SIZE 390
//#define MAX_SCREEN_WIDTH 30
//#define MAX_SCREEN_ROWS 13
volatile byte osd_buffer[MAX_SCREEN_SIZE+1];
volatile byte osd_writeOK;
// Variables for counting time and displaying bar
#define BAR_LENGTH 15
unsigned long counter=0;
unsigned long currentTime;
unsigned long cloopTime;
unsigned long bloopTime;
unsigned char barsize=0;
//////////////////////////////////////////////////////////////
void setup()
{
byte spi_junk;
int x;
pinMode(MAX7456SELECT,OUTPUT);
digitalWrite(MAX7456SELECT,HIGH); //disable device
pinMode(DATAOUT, OUTPUT);
pinMode(DATAIN, INPUT);
pinMode(SPICLOCK,OUTPUT);
// SPCR = 01010000
//interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
//sample on leading edge of clk,system clock/4 rate (4 meg)
SPCR = (1<<SPE)|(1<<MSTR);
spi_junk=SPSR;
spi_junk=SPDR;
// force soft reset on Max7456
digitalWrite(MAX7456SELECT,LOW);
spi_send_byte(MAX_VM0_REG, MAX_RESET);
digitalWrite(MAX7456SELECT,HIGH);
delay(200);
// osd_create_block_char(); // Run this once only as data is stored in eeprom
// make sure the Max7456 is enabled
digitalWrite(MAX7456SELECT,LOW);
spi_send_byte(MAX_VM0_REG, MAX_ENABLE_DISPLAY);
digitalWrite(MAX7456SELECT,HIGH);
// clear the data array
for (x = 0; x <= MAX_SCREEN_SIZE; x++)
{
osd_buffer[x] = 0x00;
}
osd_writeOK = false;
// Setup default data to display
// 30 chars x 16 lines for PAL
// Line 1 = bytes 1-30, Line2 = bytes 31-60 etc up to 480
osd_print(2,2,"Hobbytronics.co.uk");
osd_print(2,25,"OSD");
osd_print(3,24,"Test");
currentTime = millis();
cloopTime = currentTime;
bloopTime = currentTime;
osd_writeOK = true;
osd_clear_screen();
}
//////////////////////////////////////////////////////////////
void loop()
{
int i;
unsigned char hours, mins, secs;
currentTime = millis();
if(currentTime >= (cloopTime + 1000))
{
// This loop runs once a second and display the time
// in the bottom right corner
// Increment counter every second and display time since bootup
cloopTime = currentTime; // Updates cloopTime
hours = counter / 3600;
mins = (counter % 3600) / 60;
secs = counter % 60;
char ascii_str[10];
sprintf(ascii_str,"%0.2d:%0.2d:%0.2d",hours,mins,secs);
for(i=0;i<8;i++)
{
osd_buffer[440+i] = convert_ascii(ascii_str[i]);
}
counter++;
// Update line 15 only
osd_write_line(15);
}
if(currentTime >= (bloopTime + 200))
{
// This loop runs every 200mS and simply uses the block character
// we created to display a bar across the screen
// Increment bar display
// Bar displayed at line 15 col 2 = (14*30)+2
bloopTime = currentTime; // Updates cloopTime
for(i=0;i<BAR_LENGTH;i++)
{
if(i<barsize) osd_buffer[(14*30)+2+i] = 0xC0;
else osd_buffer[(14*30)+2+i] = convert_ascii(' ');;
}
barsize++;
if(barsize>=BAR_LENGTH) barsize=0;
// Update line 15
osd_write_line(15);
}
if (osd_writeOK)
{
// if osd_writeOK is set then rewrite full screen
osd_writeOK = false;
osd_write_screen();
}
}
//////////////////////////////////////////////////////////////
// Print a string to position row/col
// Places the text into the correct location in the display buffer
void osd_print(unsigned int osd_line, unsigned int osd_col, const String &s){
for (int i = 0; i < s.length(); i++) {
osd_buffer[((osd_line-1)*MAX_SCREEN_WIDTH)+osd_col+i] = convert_ascii((byte) s[i]);
}
}
//////////////////////////////////////////////////////////////
byte spi_send_byte(byte address, volatile byte data)
{
SPDR = address; // Start the transmission
while (!(SPSR & (1<<SPIF))) {}; // Wait the end of the transmission
SPDR = data; // Start the transmission
while (!(SPSR & (1<<SPIF))) {}; // Wait the end of the transmission
return SPDR; // return the received byte
}
//////////////////////////////////////////////////////////////
byte convert_ascii (int character)
{
// for some reason the MAX7456 does not follow ascii letter
// placement, so you have to have this odd lookup table
byte lookup_char;
if (character == 32)
lookup_char = 0x00; // blank space
else if (character == 48)
lookup_char = 0x0a; // 0
else if ((character > 48) && (character < 58))
lookup_char = (character - 48); // 1-9
else if ((character > 64) && (character < 90))
lookup_char = (character - 54); // A-Z
else if ((character > 96) && (character < 123))
lookup_char = (character - 60); // a-z
else if (character == 34)
lookup_char = 0x48; // "
else if (character == 39)
lookup_char = 0x46; // '
else if (character == 40)
lookup_char = 0x3f; // (
else if (character == 41)
lookup_char = 0x40; // )
else if (character == 44)
lookup_char = 0x45; // ,
else if (character == 45)
lookup_char = 0x49; // -
else if (character == 46)
lookup_char = 0x41; // .
else if (character == 47)
lookup_char = 0x47; // /
else if (character == 58)
lookup_char = 0x44; // :
else if (character == 59)
lookup_char = 0x43; // ;
else if (character == 60)
lookup_char = 0x4a; // <
else if (character == 62)
lookup_char = 0x4b; // >
else if (character == 63)
lookup_char = 0x42; // ?
else
lookup_char = 0x00; // out of range, blank space
return (lookup_char);
}
//////////////////////////////////////////////////////////////
void osd_create_block_char()
{
// Create a solid White block char at character location 0xC0
// This function only need to be called once as the character
// is stored in the MAX7456 eeprom
// Check out https://sites.google.com/site/qeewiki/projects/q-s-max7456-img-gen/QMIG_v1.10.rar?attredirects=0
// for a free character creation tool for the MAX7456
unsigned char i;
digitalWrite(MAX7456SELECT,LOW);
// disable display
spi_send_byte(MAX_VM0_REG, MAX_DISABLE_DISPLAY);
// Create Character at 0xC0 - See MAX7456 datasheet for character locations
spi_send_byte(MAX_CMAH_REG, 0xC0); // Write to address 0xC0
// We need to send 54 bytes of data to store the character. Our 'block' character
// is all the same, so we can loop here
for(i=0;i<54;i++)
{
spi_send_byte(0x0A, i);
spi_send_byte(0x0B, 0b10101010);
}
spi_send_byte(MAX_CMM_REG,0xA0);
digitalWrite(MAX7456SELECT,HIGH);
delay(200);
}
//////////////////////////////////////////////////////////////
void osd_clear_screen()
{
// clear the screen
digitalWrite(MAX7456SELECT,LOW);
spi_send_byte(MAX_DMM_REG,MAX_CLEAR_DISPLAY);
digitalWrite(MAX7456SELECT,HIGH);
}
//////////////////////////////////////////////////////////////
void osd_write_line(unsigned int linenum)
{
// Instead of redrawing the whole screen, we may only need to
// change one or more lines.
unsigned int x, local_count, start_address;
byte char_address_hi, char_address_lo;
byte osd_char;
local_count = MAX_SCREEN_WIDTH;
start_address = (linenum-1) * MAX_SCREEN_WIDTH;
digitalWrite(MAX7456SELECT,LOW);
spi_send_byte(MAX_DMM_REG,0x01); //16 bit trans w/o background
spi_send_byte(MAX_DMAH_REG,(unsigned char)(start_address>>8)); // set start address high
spi_send_byte(MAX_DMAL_REG,(unsigned char)(start_address)); // set start address low
x = 0;
while(local_count) // write out full screen
{
osd_char = osd_buffer[((linenum-1)*MAX_SCREEN_WIDTH)+x];
spi_send_byte(MAX_DMDI_REG,osd_char);
x++;
local_count--;
}
spi_send_byte(MAX_DMDI_REG,MAX_END_STRING);
digitalWrite(MAX7456SELECT,HIGH);
}
//////////////////////////////////////////////////////////////
void osd_write_screen()
{
// Change the data for the whole screen
int x, local_count;
byte char_address_hi, char_address_lo;
byte osd_char;
local_count = MAX_SCREEN_SIZE;
digitalWrite(MAX7456SELECT,LOW);
spi_send_byte(MAX_DMM_REG,0x01); //16 bit trans w/o background
spi_send_byte(MAX_DMAH_REG,0x00); // set start address high
spi_send_byte(MAX_DMAL_REG,0x00); // set start address low
x = 0;
while(local_count) // write out full screen
{
osd_char = osd_buffer[x];
spi_send_byte(MAX_DMDI_REG,osd_char);
x++;
local_count--;
}
spi_send_byte(MAX_DMDI_REG,MAX_END_STRING);
digitalWrite(MAX7456SELECT,HIGH);
}
###############################################################################################################3

BIN
configurator/gzip/index.html.gz

159
configurator/src/index.html

@ -37,7 +37,7 @@
<span class="navbar-toggler-icon"></span>
</button>
<div class="collapse navbar-collapse" id="navbarCollapse">
<ul class="navbar-nav me-auto mb-2 mb-md-0">a
<ul class="navbar-nav me-auto mb-2 mb-md-0">
<li class="nav-item">
<a class="nav-link active" aria-current="page" href="#">Dashboard</a>
</li>
@ -75,6 +75,47 @@
<div id="rc_roll" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
</div>
<div class="p-3 bd-highlight">
<h3 class="mt-5">RC Switches & Flags</h3>
<button id="rc_aux1" type="button" class="btn btn-outline-dark" disabled>AUX1</button>
<button id="rc_aux2" type="button" class="btn btn-outline-dark" disabled>AUX2</button>
<button id="rc_aux3" type="button" class="btn btn-outline-dark" disabled>AUX3</button>
<button id="rc_aux4" type="button" class="btn btn-outline-dark" disabled>AUX4</button>
<button id="rc_aux5" type="button" class="btn btn-outline-dark" disabled>AUX5</button>
<button id="rc_aux6" type="button" class="btn btn-outline-dark" disabled>AUX6</button>
<button id="rc_aux7" type="button" class="btn btn-outline-dark" disabled>AUX7</button>
<button id="rc_aux8" type="button" class="btn btn-outline-dark" disabled>AUX8</button>
<button id="rc_aux9" type="button" class="btn btn-outline-dark" disabled>AUX9</button>
</div>
<div class="p-3 bd-highlight">
<button id="ok_to_arm" type="button" class="btn btn-outline-dark" disabled>OK TO ARM</button>
<button id="armed" type="button" class="btn btn-outline-dark" disabled>ARMED</button>
<button id="acc_calibrated" type="button" class="btn btn-outline-dark" disabled>ACC CALIB</button>
<button id="mode_angle" type="button" class="btn btn-outline-dark" disabled>ANGLE</button>
<button id="mode_horizon" type="button" class="btn btn-outline-dark" disabled>HORIZON</button>
<button id="mode_mag" type="button" class="btn btn-outline-dark" disabled>MAG</button>
<button id="mode_baro" type="button" class="btn btn-outline-dark" disabled>BARO</button>
<button id="mode_headfree" type="button" class="btn btn-outline-dark" disabled>HEADFREE</button>
<button id="mode_passthru" type="button" class="btn btn-outline-dark" disabled>PASSTHRU</button>
<button id="small_angles_25" type="button" class="btn btn-outline-dark" disabled>SMALL ANGLES 25</button>
<button id="calibrate_mag" type="button" class="btn btn-outline-dark" disabled>CALIBRATE MAG</button>
<button id="mode_vario" type="button" class="btn btn-outline-dark" disabled>VARIO</button>
<button id="mode_gps" type="button" class="btn btn-outline-dark" disabled>GPS</button>
<button id="throttle_ignore" type="button" class="btn btn-outline-dark" disabled>THROTTLE IGNORE</button>
<button id="gps_fix" type="button" class="btn btn-outline-dark" disabled>GPS FIX</button>
<button id="gps_fix_home" type="button" class="btn btn-outline-dark" disabled>GPS FIX HOME</button>
<button id="gps_baro_mode" type="button" class="btn btn-outline-dark" disabled>GPS BARO MODE</button>
<button id="gps_head_set" type="button" class="btn btn-outline-dark" disabled>GPS HEAD SET</button>
<button id="land_complete" type="button" class="btn btn-outline-dark" disabled>LAND COMPLETED</button>
<button id="land_in_progress" type="button" class="btn btn-outline-dark" disabled>LAND IN PROGRESS</button>
<button id="gps_fix" type="button" class="btn btn-outline-dark" disabled>GPS FIX</button>
</div>
<div class="p-3 bd-example">
<h3 class="mt-5">Profile Settings</h3>
<button id="profile_1" type="button" class="btn btn-outline-dark" disabled>PROFILE 1</button>
<button id="profile_2" type="button" class="btn btn-outline-dark" disabled>PROFILE 2</button>
<button id="profile_3" type="button" class="btn btn-outline-dark" disabled>PROFILE 3</button>
</div>
<div class="p-3 bd-highlight">
<h3 class="mt-5">PID Configurations</h3>
<table class="table table-dark table-striped">
@ -157,38 +198,7 @@
</tr>
</tbody>
</table>
</div>
<div class="p-2 bd-highlight">
<h3 class="mt-5">Switches</h3>
<table class="table table-striped">
<thead>
<tr>
<th scope="col">#</th>
<th scope="col">First</th>
<th scope="col">Last</th>
<th scope="col">Handle</th>
</tr>
</thead>
<tbody>
<tr>
<th scope="row">1</th>
<td>Mark</td>
<td>Otto</td>
<td>@mdo</td>
</tr>
<tr>
<th scope="row">2</th>
<td>Jacob</td>
<td>Thornton</td>
<td>@fat</td>
</tr>
<tr>
<th scope="row">3</th>
<td colspan="2">Larry the Bird</td>
<td>@twitter</td>
</tr>
</tbody>
</table>
<button id="pid_button" type="button" class="btn btn-dark">UPDATE</button>
</div>
</main>
@ -200,7 +210,9 @@
</footer>
<script>
var i = 0;
var current_profile_id = 0;
var busy = 0;
setInterval(
function() {
get_feeds();
@ -208,7 +220,15 @@
);
get_pids();
// Handling button clicks
$('#pid_button').on('click', function(event) {
alert("OK");
});
function get_feeds() {
// Making sure 1 thread is active only...
if(busy == 1) return;
var url = "http://192.168.1.1/feeds";
$.get(url, function(data) {
// console.log("Feeds: " + data);
@ -222,13 +242,78 @@
$('#rc_yaw').css('width',calc_rc_percent(feeds[1])+'%');
$('#rc_yaw').html(feeds[1]);
$('#rc_pitch').css('width',calc_rc_percent(feeds[2])+'%');
$('#rc_pitch').html(feeds[2]);
$('#Rc_pitch').html(feeds[2]);
$('#rc_roll').css('width',calc_rc_percent(feeds[3])+'%');
$('#rc_roll').html(feeds[3]);
// RC Switches and Flags
(feeds[4] == 1) ? $("#rc_aux1").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux1").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[5] == 1) ? $("#rc_aux2").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux2").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[6] == 1) ? $("#rc_aux3").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux3").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[7] == 1) ? $("#rc_aux4").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux4").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[8] == 1) ? $("#rc_aux5").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux5").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[9] == 1) ? $("#rc_aux6").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux6").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[10] == 1) ? $("#rc_aux7").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux7").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[11] == 1) ? $("#rc_aux8").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux8").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[12] == 1) ? $("#rc_aux9").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#rc_aux9").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[13] == 1) ? $("#ok_to_arm").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#ok_to_arm").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[14] == 1) ? $("#armed").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#armed").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[15] == 1) ? $("#acc_calibrated").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#acc_calibrated").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[16] == 1) ? $("#mode_angle").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_angle").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[17] == 1) ? $("#mode_horizon").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_horizon").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[18] == 1) ? $("#mode_mag").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_mag").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[19] == 1) ? $("#mode_baro").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_baro").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[20] == 1) ? $("#mode_headfree").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_headfree").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[21] == 1) ? $("#mode_passthru").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_passthru").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[22] == 1) ? $("#small_angles_25").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#small_angles_25").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[23] == 1) ? $("#calibrate_mag").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#calibrate_mag").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[24] == 1) ? $("#mode_vario").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_vario").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[25] == 1) ? $("#mode_gps").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#mode_gps").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[26] == 1) ? $("#throttle_ignore").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#throttle_ignore").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[27] == 1) ? $("#gps_fix").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#gps_fix").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[28] == 1) ? $("#gps_fix_home").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#gps_fix_home").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[29] == 1) ? $("#gps_baro_mode").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#gps_baro_mode").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[30] == 1) ? $("#gps_head_set").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#gps_head_set").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[31] == 1) ? $("#land_complete").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#land_complete").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[32] == 1) ? $("#land_in_progress").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#land_in_progress").removeClass("btn btn-success").addClass("btn btn-outline-dark");
profile_set(feeds[33]);
});
}
// Display profile based on data feed
function profile_set(profile_id) {
if(profile_id == 0) {
$("#profile_1").removeClass("btn btn-outline-dark").addClass("btn btn-success");
$("#profile_2").removeClass("btn btn-success").addClass("btn btn-outline-dark");
$("#profile_3").removeClass("btn btn-success").addClass("btn btn-outline-dark");
if(current_profile_id != 0) {
current_profile_id = 0;
get_pids();
}
} else if(profile_id == 1) {
$("#profile_1").removeClass("btn btn-success").addClass("btn btn-outline-dark");
$("#profile_2").removeClass("btn btn-outline-dark").addClass("btn btn-success");
$("#profile_3").removeClass("btn btn-success").addClass("btn btn-outline-dark");
if(current_profile_id != 1) {
current_profile_id = 1;
get_pids();
}
} else if(profile_id == 2) {
$("#profile_1").removeClass("btn btn-success").addClass("btn btn-outline-dark");
$("#profile_2").removeClass("btn btn-success").addClass("btn btn-outline-dark");
$("#profile_3").removeClass("btn btn-outline-dark").addClass("btn btn-success");
if(current_profile_id != 2) {
current_profile_id = 2;
get_pids();
}
}
}
// Convert 1000 ~ 2000 into percentage
function calc_rc_percent(data) {
if(data == 0 || data < 1000) return 0;
@ -237,6 +322,8 @@
function get_pids() {
busy = 1;
var url = "http://192.168.1.1/getpids";
$.get(url, function(data) {
console.log("Data: " + data);
@ -282,6 +369,8 @@
// $('#angle_trim_roll').val(pids[34]);
// $('#angle_trim_pitch').val(pids[35]);
});
busy = 0;
}
</script>

2
getFeeds.sh

@ -1,2 +1,2 @@
#!/bin/bash
curl http://192.168.1.1/feeds; echo
curl -s http://192.168.1.1/feeds; echo

3
src/Motors.cpp

@ -79,3 +79,6 @@ void Motors::mix_table() {
}
}
Motors motors = Motors(MOTOR1, MOTOR2, MOTOR3, MOTOR4);

2
src/Motors.h

@ -17,4 +17,6 @@ class Motors {
uint8_t _motor1_pin, _motor2_pin, _motor3_pin, _motor4_pin;
};
extern Motors motors;
#endif

170
src/gyro.cpp

@ -63,7 +63,10 @@ void GYRO::begin(void) {
// Serial.print("Status SPI Gyro (WHO_AM_I): "); Serial.println(status);
delay(100);
roll = 0;
pitch = 0;
yaw = 0;
// delayMicroseconds(5);
// digitalWrite(GYRO_CS, HIGH); // Temporary set to unselect
@ -74,10 +77,6 @@ void GYRO::begin(void) {
}
uint16_t GYRO::get_readings(void) {
}
uint8_t GYRO::get_id(void) {
uint8_t status = read_register(WHO_AM_I);
Serial.print("Status SPI Gyro (WHO_AM_I): "); Serial.println(status);
@ -156,6 +155,59 @@ void GYRO::acc_gyro_init(void) {
write_register(INT_ENABLE, BIT_RAW_RDY_EN);
delay(15);
#endif
calculate_imu_error();
delay(20);
}
void GYRO::calculate_imu_error(void) {
uint8_t c = 0;
// We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
// Read accelerometer values 200 times
while(c < 200) {
AcX = read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L);
AcY = read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L);
AcZ = read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L);
AccErrorX = AccErrorX + ((atan((AcY) / sqrt(pow((AcX), 2) + pow((AcZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AcX) / sqrt(pow((AcY), 2) + pow((AcZ), 2))) * 180 / PI));
c++;
}
// Divide the sum by 200 to get the error value
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Read gyro values 200 times
while(c < 200) {
GyX = read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L);
GyY = read_register(GYRO_YOUT_H) << 8 | read_register(GYRO_YOUT_L);
GyZ = read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L);
// Sum all readings
GyroErrorX = GyroErrorX + (GyX / 131.0);
GyroErrorY = GyroErrorY + (GyY / 131.0);
GyroErrorZ = GyroErrorZ + (GyZ / 131.0);
c++;
}
// Divide the sum by 200 to get the error value
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
#ifdef DEBUG_MODE
sprintf(debug_str,
"Acc:\nX: %.4f, Y: %.4f\n\nGyro:\nX: %.4f, Y: %.4f, Z: %.4f\n",
AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ);
Serial.println(debug_str);
#endif
}
@ -164,9 +216,17 @@ void GYRO::read_all(void) {
AcY = read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L);
AcZ = read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L);
// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
GyX = read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L);
GyY = read_register(GYRO_YOUT_H) << 8 | read_register(GYRO_YOUT_L);
GyZ = read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L);
temperatureG = read_register(TEMP_OUT_L);
}
@ -221,4 +281,104 @@ float GYRO::InvSqrt(float x){
return conv.f * (1.68191409f - 0.703952253f * x * conv.f * conv.f);
}
void GYRO::calculate_angles_beta(void) {
// float gyroRoll = roll + gyroDegPerSec(GyX, GYRO_FS) * DELTA_TIME;
}
// Old multiwii code uses void getEstimatedAltitude();
void GYRO::calculate_angles(void) {
uint8_t axis;
int32_t accMag = 0;
float scale;
int16_t delayGyroAngle16[3];
static t_int32_t_vector EstG = { 0, 0, (int32_t) ACC_1G << 16};
#if MAG
static t_int32_t_vector EstM;
#else
static t_int32_t_vector EstM = { 0, (int32_t)1 << 24, 0};
#endif
static uint32_t LPFAcc[3];
float invG; // 1/|G|
static int16_t accZoffset = 0;
int32_t accZ_tmp = 0;
static uint16_t previousT;
uint16_t currentT = micros();
// unit: radian per bit, scaled by 2^16 for further multiplication
// with a delta time of 3000 us, and GYRO scale of most gyros, scale = a little bit less than 1
scale = (currentT - previousT) * (GYRO_SCALE * 65536);
previousT = currentT;
// Initialization
for(axis = 0; axis < 3; axis++) {
// valid as long as LPF_FACTOR is less than 15
imu.accSmooth[axis] = LPFAcc[axis]>>ACC_LPF_FACTOR;
LPFAcc[axis] += imu.accADC[axis] - imu.accSmooth[axis];
// used to calculate later the magnitude of acc vector
accMag += (imu.accSmooth[axis] * imu.accSmooth[axis]);
// unit: radian scaled by 2^16
// imu.gyroADC[axis] is 14 bit long, the scale factor ensure deltaGyroAngle16[axis] is still 14 bit long
deltaGyroAngle16[axis] = imu.gyroADC[axis] * scale;
}
// we rotate the intermediate 32 bit vector with the radian vector (deltaGyroAngle16), scaled by 2^16
// however, only the first 16 MSB of the 32 bit vector is used to compute the result
// it is ok to use this approximation as the 16 LSB are used only for the complementary filter part
rotateV32(&EstG, deltaGyroAngle16);
rotateV32(&EstM, deltaGyroAngle16);
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
for(axis = 0; axis < 3; axis++) {
if((int16_t)(0.85 * ACC_1G * ACC_1G / 256) < (int16_t)(accMag >> 8) && (int16_t)(accMag >> 8) < (int16_t)(1.15 * ACC_1G * ACC_1G / 256))
EstG.A32[axis] += (int32_t)(imu.accSmooth[axis] - EstG.A16[2*axis+1]) << (16 - GYR_CMPF_FACTOR);
accZ_tmp += (imu.accSmooth[axis] * EstG.A16[2*axis+1]);
#if MAG
EstM.A32[axis] += (int32_t)(imu.magADC[axis] - EstM.A16[2 * axis + 1]) << (16 - GYR_CMPFM_FACTOR);
#endif
}
if(EstG.V16.Z > ACCZ_25deg)
flags.SMALL_ANGLES_25 = 1;
else
flags.SMALL_ANGLES_25 = 0;
// Attitude of the estimated vector
int32_t sqGX_sqGZ = (EstG.V16.X * EstG.V16.X) + (EstG.V16.Z * EstG.V16.Z);
invG = InvSqrt(sqGX_sqGZ + (EstG.V16.Y * EstG.V16.Y));
att.angle[ROLL] = _atan2(EstG.V16.X , EstG.V16.Z);
att.angle[PITCH] = _atan2(EstG.V16.Y , InvSqrt(sqGX_sqGZ) * sqGX_sqGZ);
// note on the second term: mathematically there is a risk of overflow (16*16*16=48 bits). assumed to be null with real values
att.heading = _atan2(
(EstM.V16.Z * EstG.V16.X) - (EstM.V16.X * EstG.V16.Z),
(EstM.V16.Y * sqGX_sqGZ - ((EstM.V16.X * EstG.V16.X) + (EstM.V16.Z * EstG.V16.Z)) * EstG.V16.Y) * invG);
#if MAG
att.heading += conf.mag_declination; // Set from GUI
#endif
att.heading /= 10;
#if defined(THROTTLE_ANGLE_CORRECTION)
cosZ = (EstG.V16.Z * 100) / ACC_1G; // cos(angleZ) * 100
throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >> 3; // 16 bit ok: 200*150 = 30000
#endif
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
accZ = accZ_tmp * invG;
if(!flags.ARMED) {
accZoffset -= accZoffset >> 3;
accZoffset += accZ;
}
accZ -= accZoffset >> 3;
}
GYRO gyro;

21
src/gyro.h

@ -29,7 +29,17 @@
#define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = X; imu.accADC[PITCH] = Y; imu.accADC[YAW] = Z;}
#endif
#define RAD_TO_DEGREE 57.2957795131
#define SAMPLE_RATE 250 // 250 Hz
#define DELTA_TIME 1.0f / SAMPLE_RATE
#define GYROFS 1
#define GYR_CMPF_FACTOR 10
#define ACC_LPF_FACTOR 4
#define ACC_VelScale (9.80665f / 10000.0f / ACC_1G)
// #define GYRO_SCALE (4 / 16.4 * PI / 180.0 / 1000000.0) // 16.4 LSB = 1 deg/s
#define GYRO_SCALE (1.0f / 16.4f)
#define ACCZ_25deg (int16_t)(ACC_1G * 0.90631) // 0.90631 = cos(25deg) (cos(theta) of accZ comparison)
// Registry Address
// #define XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
@ -202,7 +212,6 @@ class GYRO {
void begin(void);
void acc_gyro_init(void);
uint8_t get_id(void);
uint16_t get_readings(void);
void beginTransaction(void);
void endTransaction(void);
uint8_t read_register(uint8_t addr);
@ -211,6 +220,16 @@ class GYRO {
void rotateV32(t_int32_t_vector *v, int16_t* delta);
float InvSqrt(float x);
int16_t _atan2(int32_t y, int32_t x);
void calculate_angles(void);
void calculate_angles_beta(void);
void calculate_imu_error(void);
int16_t deltaGyroAngle16[3];
int16_t accZ = 0;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float accAngleX, accAngleY;
private:
};

18
src/main.cpp

@ -17,7 +17,7 @@ BARO baro;
// TASK task;
// Setting up Motors
Motors motors = Motors(MOTOR1, MOTOR2, MOTOR3, MOTOR4);
// Motors motors = Motors(MOTOR1, MOTOR2, MOTOR3, MOTOR4);
RXMessage rxMessage;
// Global Variables
@ -179,6 +179,8 @@ int16_t baroTemperature;
int32_t baroPressureSum;
#endif
uint8_t temperatureG = 0;
alt_t alt;
att_t att;
imu_t imu;
@ -609,7 +611,9 @@ void taskMeasurements(void *pvParameters) {
// sprintf(buffer_gyro, "Gyro: %d %d %d %d %d %d Cycle: %d Bat: %.2fv Temp: %.2fC Alt: %dcm T: %d Y: %d P: %d R: %d", AcX, AcY, AcZ, GyX, GyY, GyZ, cycleTime, battery.volt, baro.temperature, baro.altitude, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL]);
sprintf(buffer_gyro, "Gyro: %d %d %d %d %d %d Cycle: %d Bat: %.2fv Temp: %.2fC Alt: %dcm T: %d Y: %d P: %d R: %d", AcX, AcY, AcZ, GyX, GyY, GyZ, cycleTime, battery.volt, baro.temperature, alt.EstAlt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL]);
#else
sprintf(buffer_gyro, "Gyro: %d %d %d %d %d %d Motor: %d Cycle: %d RC: %d Bat: %.2fv T: %d Y: %d P: %d R: %d FS: %d", AcX, AcY, AcZ, GyX, GyY, GyZ, motors.counts, cycleTime, rc_counts, battery.volt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL], failsafeCnt);
sprintf(buffer_gyro,
"Gyro: %d %d %d %d %d %d Temperature: %d Motor: %d Cycle: %d RC: %d Bat: %.2fv T: %d Y: %d P: %d R: %d FS: %d\nAccAngleX: %.4f, AccAngleY: %.4f",
AcX, AcY, AcZ, GyX, GyY, GyZ, temperatureG, motors.counts, cycleTime, rc_counts, battery.volt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL], failsafeCnt, gyro.accAngleX, gyro.accAngleY);
#endif
// Reset counter
@ -1075,10 +1079,10 @@ bool process_rc(void) {
void compute_imu(void) {
timeInterleave = 0;
#if USE_BARO
////// #if USE_BARO
///// baro.get_readings();
baro.getEstimatedAltitude();
#endif
///// baro.getEstimatedAltitude();
///// #endif
// Reading ACC and Gyro values
gyro.read_all();
@ -1086,6 +1090,10 @@ void compute_imu(void) {
// IMU.cpp at multiwii....
ACC_ORIENTATION(AcX, AcY, AcZ);
acc_common();
// Calcualtions
gyro.calculate_angles_beta();
GYRO_ORIENTATION(GyX, GyY, GyZ); // Range +/- 8192; +/- 2000 deg/sec?
gyro_common();

24
src/main.h

@ -112,31 +112,19 @@ typedef struct {
uint8_t HORIZON_MODE :1 ;
uint8_t MAG_MODE :1 ;
uint8_t BARO_MODE :1 ;
#ifdef HEADFREE
uint8_t HEADFREE_MODE :1 ;
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
uint8_t PASSTHRU_MODE :1 ;
#endif
uint8_t SMALL_ANGLES_25 :1 ;
#if MAG
uint8_t CALIBRATE_MAG :1 ;
#endif
#ifdef VARIOMETER
uint8_t VARIO_MODE :1;
#endif
uint8_t GPS_mode: 2; // 0-3 NONE,HOLD, HOME, NAV (see GPS_MODE_* defines
#if USE_BARO || GPS
uint8_t THROTTLE_IGNORED : 1; // If it is 1 then ignore throttle stick movements in baro mode;
#endif
#if GPS
uint8_t GPS_FIX :1 ;
uint8_t GPS_FIX_HOME :1 ;
uint8_t GPS_BARO_MODE : 1; // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
uint8_t GPS_head_set: 1; // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
uint8_t LAND_COMPLETED: 1;
uint8_t LAND_IN_PROGRESS: 1;
#endif
} flags_struct_t;
@ -155,7 +143,7 @@ enum box {
BOXMAG,
#if defined(HEADFREE)
BOXHEADFREE,
BOXHEADADJ, // acquire heading for HEADFREE mode
BOXHEADADJ, // acquire heading for HEADFREE mode
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(SERVO_MIX_TILT)
BOXCAMSTAB,
@ -213,6 +201,7 @@ typedef struct {
uint8_t checksum; // MUST BE ON LAST POSITION OF STRUCTURE !
} global_conf_t;
#ifdef LOG_PERMANENT
typedef struct {
uint16_t arm; // #arm events
@ -522,6 +511,7 @@ extern conf_t conf;
extern uint16_t failsafeCnt;
extern imu_t imu;
extern int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
extern uint8_t temperatureG;
extern int32_t baroPressure;
extern int16_t baroTemperature;
extern int32_t baroPressureSum;
@ -537,9 +527,17 @@ extern int16_t lookupPitchRollRC[5]; // lookup table for expo & RC rate PITCH+
extern uint16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
extern int16_t failsafeEvents;
extern analog_t analog;
extern uint16_t cycleTime; // This is the number in micro second to achieve a full loop, it can differ a little and is taken
extern uint32_t rc_counts;
extern alt_t alt;
extern att_t att;
extern imu_t imu;
#ifdef LOG_PERMANENT
extern plog_t plog;
#endif
#ifdef DEBUG_MODE
extern char debug_str[512];
#endif
// Headers
#include "wifi.h"

101
src/web.cpp

@ -327,52 +327,63 @@ void WEB::begin(void) {
server.on("/feeds", HTTP_GET, []() {
String feeds = "";
feeds =
String(rcData[THROTTLE]) + "," +
String(rcData[YAW]) + "," +
String(rcData[PITCH]) + "," +
String(rcData[ROLL]) + "," +
String(rcData[AUX1]) + "," +
String(rcData[AUX2]) + "," +
String(rcData[AUX3]) + "," +
String(rcData[AUX4]) + "," +
String(rcData[AUX5]) + "," +
String(rcData[AUX6]) + "," +
String(rcData[AUX7]) + "," +
String(rcData[AUX8]) + "," +
String(rcData[AUX9]) + "," +
String(rcData[THROTTLE]) + "," +
String(rcData[YAW]) + "," +
String(rcData[PITCH]) + "," +
String(rcData[ROLL]) + "," +
String(rcData[AUX1]) + "," +
String(rcData[AUX2]) + "," +
String(rcData[AUX3]) + "," +
String(rcData[AUX4]) + "," +
String(rcData[AUX5]) + "," +
String(rcData[AUX6]) + "," +
String(rcData[AUX7]) + "," +
String(rcData[AUX8]) + "," +
String(rcData[AUX9]) + "," +
// Checking the flags
String(flags.OK_TO_ARM) + "," +
String(flags.ARMED) + "," +
String(flags.ACC_CALIBRATED) + "," +
String(flags.ANGLE_MODE) + "," +
String(flags.HORIZON_MODE) + "," +
String(flags.MAG_MODE) + "," +
String(flags.BARO_MODE) + "," +
#ifdef HEADFREE
String(flags.HEADFREE_MODE) + "," +
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
String(flags.PASSTHRU_MODE) + "," +
#endif
String(flags.SMALL_ANGLES_25)+ "," +
#if MAG
String(flags.CALIBRATE_MAG) + "," +
#endif
#ifdef VARIOMETER
String(flags.VARIO_MODE) + "," +
#endif
String(flags.GPS_mode) + "," +
#if USE_BARO || GPS
String(flags.THROTTLE_IGNORED) + "," +
#endif
#if GPS
String(flags.GPS_FIX) + "," +
String(flags.GPS_FIX_HOME) + "," +
String(flags.GPS_BARO_MODE) + "," +
String(flags.GPS_head_set) + "," +
String(flags.LAND_COMPLETED) + "," +
String(flags.LAND_IN_PROGRESS) + "," +
#endif
String(flags.OK_TO_ARM) + "," +
String(flags.ARMED) + "," +
String(flags.ACC_CALIBRATED) + "," +
String(flags.ANGLE_MODE) + "," +
String(flags.HORIZON_MODE) + "," +
String(flags.MAG_MODE) + "," +
String(flags.BARO_MODE) + "," +
String(flags.HEADFREE_MODE) + "," +
String(flags.PASSTHRU_MODE) + "," +
String(flags.SMALL_ANGLES_25) + "," +
String(flags.CALIBRATE_MAG) + "," +
String(flags.VARIO_MODE) + "," +
String(flags.GPS_mode) + "," +
String(flags.THROTTLE_IGNORED) + "," +
String(flags.GPS_FIX) + "," +
String(flags.GPS_FIX_HOME) + "," +
String(flags.GPS_BARO_MODE) + "," +
String(flags.GPS_head_set) + "," +
String(flags.LAND_COMPLETED) + "," +
String(flags.LAND_IN_PROGRESS) + "," +
// Global configs
String(global_conf.currentSet) + "," +
String(global_conf.accZero[ROLL]) + "," +
String(global_conf.accZero[PITCH]) + "," +
String(global_conf.accZero[YAW]) + "," +
String(global_conf.magZero[ROLL]) + "," +
String(global_conf.magZero[PITCH]) + "," +
String(global_conf.magZero[YAW]) + "," +
// Gyro, Cycle, RX Rate, Battery, Motor Refreshing Rate
String(AcX) + "," +
String(AcY) + "," +
String(AcZ) + "," +
String(GyX) + "," +
String(GyY) + "," +
String(GyZ) + "," +
String(cycleTime) + "," +
String(rc_counts) + "," +
String(battery.volt) + "," +
String(motors.counts) + "," +
// Angle and Heading...
String(att.angle[ROLL]) + "," +
String(att.angle[PITCH]) + "," +
String(att.heading) + "," +
"OK";

Loading…
Cancel
Save