#include "dryos.h"
#include "bmp.h"
#include "tasks.h"
#include "debug.h"
#include "math.h"
#include "menu.h"
#include "property.h"
#include "config.h"
#include "gui.h"
#include "lens.h"
#include "version.h"
#include "edmac.h"
#include "asm.h"
#include "lvinfo.h"
#ifdef CONFIG_ELECTRONIC_LEVEL
/* Canon stub */
extern void GUI_SetRollingPitchingLevelStatus(int request);
struct rolling_pitching
{
uint8_t status;
uint8_t cameraposture;
uint8_t roll_sensor1;
uint8_t roll_sensor2;
uint8_t pitch_sensor1;
uint8_t pitch_sensor2;
};
static struct rolling_pitching level_data;
PROP_HANDLER(PROP_ROLLING_PITCHING_LEVEL)
{
memcpy(&level_data, buf, 6);
}
static void draw_level_lines(int angle, int pitch, int show)
{
int x0 = os.x0 + os.x_ex/2;
int y0 = os.y0 + os.y_ex/2;
int r = 200;
#define MUL 16384
#define PI_1800 0.00174532925
// Compute roll line parameters
int s = sinf(angle * PI_1800) * MUL;
int c = cosf(angle * PI_1800) * MUL;
int x_offset = r * c / MUL;
int y_offset = r * s / MUL;
int dx = (angle % 1800 >= 450 && angle % 1800 < 1350);
int dy = 1 - dx;
int color1 = show ? ((angle % 900) ? COLOR_BLACK : COLOR_GREEN1) : 0;
int color2 = show ? ((angle % 900) ? COLOR_WHITE : COLOR_GREEN2) : 0;
// Draw or erase the roll line
draw_line(x0 - x_offset, y0 - y_offset, x0 + x_offset, y0 + y_offset, color1);
draw_line(x0 - x_offset + dx, y0 - y_offset + dy, x0 + x_offset + dx, y0 + y_offset + dy, color2);
// Don't draw the pitch line until we see the pitch value change. This prevents it
// ever being drawn on cameras that don't report pitch, such as the 60D.
static int pitch_changed = 0;
if (pitch != 0) pitch_changed = 1;
if (!pitch_changed) return;
// Compute pitch line parameters
int pitch_s = sinf(pitch * PI_1800) * MUL;
r = 120;
x0 -= (r * pitch_s / MUL) * s / MUL;
y0 += (r * pitch_s / MUL) * c / MUL;
x_offset /= 2;
y_offset /= 2;
color1 = show ? ((pitch % 900) ? COLOR_BLACK : COLOR_GREEN1) : 0;
color2 = show ? ((pitch % 900) ? COLOR_WHITE : COLOR_GREEN2) : 0;
// Draw or erase the pitch line
draw_line(x0 - x_offset, y0 - y_offset, x0 + x_offset, y0 + y_offset, color1);
draw_line(x0 - x_offset + dx, y0 - y_offset + dy, x0 + x_offset + dx, y0 + y_offset + dy, color2);
}
static void draw_electronic_level(int angle, int prev_angle, int pitch, int prev_pitch, int force_redraw)
{
if (!force_redraw && angle == prev_angle && pitch == prev_pitch) return;
draw_level_lines(prev_angle, prev_pitch, 0);
draw_level_lines(angle, pitch, 1);
}
void disable_electronic_level()
{
if (level_data.status == 2)
{
GUI_SetRollingPitchingLevelStatus(1);
msleep(100);
}
}
void show_electronic_level()
{
static int prev_angle10 = 0;
static int prev_pitch10 = 0;
int force_redraw = 0;
if (level_data.status != 2)
{
GUI_SetRollingPitchingLevelStatus(0);
msleep(100);
force_redraw = 1;
}
static int k = 0;
k++;
if (k % 10 == 0) force_redraw = 1;
int angle100 = level_data.roll_sensor1 * 256 + level_data.roll_sensor2;
int angle10 = angle100/10;
int pitch100 = level_data.pitch_sensor1 * 256 + level_data.pitch_sensor2;
int pitch10 = pitch100/10;
draw_electronic_level(angle10, prev_angle10, pitch10, prev_pitch10, force_redraw);
prev_angle10 = angle10;
prev_pitch10 = pitch10;
}
#define FMT_FIXEDPOINT1E(x) (x) < 0 ? "-" : (x) > 0 ? "+" : "=", ABS(x)/10, ABS(x)%10
static LVINFO_UPDATE_FUNC(electronic_level_update)
{
LVINFO_BUFFER(8);
if(level_data.status == 2)
{
int angle10 = (level_data.roll_sensor1 * 256 + level_data.roll_sensor2) / 10;
if (angle10 > 1800) angle10 -= 3600;
snprintf(buffer, sizeof(buffer), "%s%d.%d" SYM_DEGREE, FMT_FIXEDPOINT1E(angle10));
item->color_fg = angle10 == 0 || ABS(angle10) == 900 || ABS(angle10) == 1800 ? COLOR_GREEN1 : COLOR_WHITE;
}
}
static struct lvinfo_item info_items[] = {
{
.name = "Electronic level",
.which_bar = LV_TOP_BAR_ONLY,
.update = electronic_level_update,
.preferred_position = -100,
.priority = 1,
},
};
static void electronic_level_init()
{
lvinfo_add_items(info_items, COUNT(info_items));
}
INIT_FUNC("electronic_level_info", electronic_level_init);
#endif