rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

POI orbit + some code run thru code formatter

Browse Source
This commit is contained in:
sambas 2013-04-14 12:23:47 +03:00
parent 6989468d17
commit 589290dbd7
8 changed files with 3082 additions and 3189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

286
flight/Libraries/paths.c
View File

@ -32,8 +32,8 @@
// no direct UAVObject usage allowed in this file // no direct UAVObject usage allowed in this file
// private functions // private functions
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status); static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status); static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise); static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
/** /**
@ -46,26 +46,26 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
*/ */
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode) void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
{ {
switch(mode) { switch (mode) {
case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status); return path_vector(start_point, end_point, cur_point, status);
break; break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1); return path_circle(start_point, end_point, cur_point, status, 1);
break; break;
case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0); return path_circle(start_point, end_point, cur_point, status, 0);
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT:
default: default:
// use the endpoint as default failsafe if called in unknown modes // use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status); return path_endpoint(start_point, end_point, cur_point, status);
break; break;
} }
} }
/** /**
@ -75,38 +75,38 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
* @param[in] cur_point Current location * @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation * @param[out] status Structure containing progress along path and deviation
*/ */
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status) static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status)
{ {
float path_north, path_east, diff_north, diff_east; float path_north, path_east, diff_north, diff_east;
float dist_path, dist_diff; float dist_path, dist_diff;
// we do not correct in this mode // we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0; status->correction_direction[0] = status->correction_direction[1] = 0;
// Distance to go // Distance to go
path_north = end_point[0] - start_point[0]; path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1]; path_east = end_point[1] - start_point[1];
// Current progress location relative to end // Current progress location relative to end
diff_north = end_point[0] - cur_point[0]; diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1]; diff_east = end_point[1] - cur_point[1];
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east ); dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
dist_path = sqrtf( path_north * path_north + path_east * path_east ); dist_path = sqrtf(path_north * path_north + path_east * path_east);
if(dist_diff < 1e-6 ) { if (dist_diff < 1e-6) {
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = 0; status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0; status->path_direction[0] = status->path_direction[1] = 0;
return; return;
} }
status->fractional_progress = 1 - dist_diff / (1 + dist_path); status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->error = dist_diff; status->error = dist_diff;
// Compute direction to travel // Compute direction to travel
status->path_direction[0] = diff_north / dist_diff; status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff; status->path_direction[1] = diff_east / dist_diff;
} }
@ -117,50 +117,50 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
* @param[in] cur_point Current location * @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation * @param[out] status Structure containing progress along path and deviation
*/ */
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status) static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status)
{ {
float path_north, path_east, diff_north, diff_east; float path_north, path_east, diff_north, diff_east;
float dist_path; float dist_path;
float dot; float dot;
float normal[2]; float normal[2];
// Distance to go // Distance to go
path_north = end_point[0] - start_point[0]; path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1]; path_east = end_point[1] - start_point[1];
// Current progress location relative to start // Current progress location relative to start
diff_north = cur_point[0] - start_point[0]; diff_north = cur_point[0] - start_point[0];
diff_east = cur_point[1] - start_point[1]; diff_east = cur_point[1] - start_point[1];
dot = path_north * diff_north + path_east * diff_east; dot = path_north * diff_north + path_east * diff_east;
dist_path = sqrtf( path_north * path_north + path_east * path_east ); dist_path = sqrtf(path_north * path_north + path_east * path_east);
if(dist_path < 1e-6) { if (dist_path < 1e-6) {
// if the path is too short, we cannot determine vector direction. // if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away, // Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way. // but assume progress=1 either way.
path_endpoint( start_point, end_point, cur_point, status ); path_endpoint(start_point, end_point, cur_point, status);
status->fractional_progress = 1; status->fractional_progress = 1;
return; return;
} }
// Compute the normal to the path // Compute the normal to the path
normal[0] = -path_east / dist_path; normal[0] = -path_east / dist_path;
normal[1] = path_north / dist_path; normal[1] = path_north / dist_path;
status->fractional_progress = dot / (dist_path * dist_path); status->fractional_progress = dot / (dist_path * dist_path);
status->error = normal[0] * diff_north + normal[1] * diff_east; status->error = normal[0] * diff_north + normal[1] * diff_east;
// Compute direction to correct error // Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
// Now just want magnitude of error
status->error = fabs(status->error);
// Compute direction to travel // Now just want magnitude of error
status->path_direction[0] = path_north / dist_path; status->error = fabs(status->error);
status->path_direction[1] = path_east / dist_path;
// Compute direction to travel
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
} }
@ -173,77 +173,79 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
*/ */
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise) static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
{ {
float radius_north, radius_east, diff_north, diff_east; float radius_north, radius_east, diff_north, diff_east;
float radius,cradius; float radius, cradius;
float normal[2]; float normal[2];
float progress; float progress;
float a_diff, a_radius; float a_diff, a_radius;
// Radius // Radius
radius_north = end_point[0] - start_point[0]; radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1]; radius_east = end_point[1] - start_point[1];
// Current location relative to center // Current location relative to center
diff_north = cur_point[0] - end_point[0]; diff_north = cur_point[0] - end_point[0];
diff_east = cur_point[1] - end_point[1]; diff_east = cur_point[1] - end_point[1];
radius = sqrtf( powf(radius_north,2) + powf(radius_east,2) ); radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
cradius = sqrtf( powf(diff_north,2) + powf(diff_east,2) ); cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
if (cradius < 1e-6) { if (cradius < 1e-6) {
// cradius is zero, just fly somewhere and make sure correction is still a normal // cradius is zero, just fly somewhere and make sure correction is still a normal
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = radius; status->error = radius;
status->correction_direction[0] = 0; status->correction_direction[0] = 0;
status->correction_direction[1] = 1; status->correction_direction[1] = 1;
status->path_direction[0] = 1; status->path_direction[0] = 1;
status->path_direction[1] = 0; status->path_direction[1] = 0;
return; return;
} }
if (clockwise) { if (clockwise) {
// Compute the normal to the radius clockwise // Compute the normal to the radius clockwise
normal[0] = -diff_east / cradius; normal[0] = -diff_east / cradius;
normal[1] = diff_north / cradius; normal[1] = diff_north / cradius;
} else { } else {
// Compute the normal to the radius counter clockwise // Compute the normal to the radius counter clockwise
normal[0] = diff_east / cradius; normal[0] = diff_east / cradius;
normal[1] = -diff_north / cradius; normal[1] = -diff_north / cradius;
} }
// normalize progress to 0..1 // normalize progress to 0..1
a_diff = atan2f( diff_north, diff_east); a_diff = atan2f(diff_north, diff_east);
a_radius = atan2f( radius_north, radius_east); a_radius = atan2f(radius_north, radius_east);
if(a_diff<0)
a_diff+=2*M_PI;
if(a_radius<0)
a_radius+=2*M_PI;
progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
if(progress<0) if (a_diff < 0) {
progress+=1.0; a_diff += 2 * M_PI;
else if(progress>=1) }
progress-=1.0; if (a_radius < 0) {
a_radius += 2 * M_PI;
}
if(clockwise) progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
{
progress=1-progress;
}
status->fractional_progress = progress;
// error is current radius minus wanted radius - positive if too close if (progress < 0) {
status->error = radius - cradius; progress += 1.0;
} else if (progress >= 1) {
progress -= 1.0;
}
// Compute direction to correct error if (clockwise) {
status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius; progress = 1 - progress;
status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius; }
// Compute direction to travel status->fractional_progress = progress;
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->error = fabs(status->error); // error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->error = fabs(status->error);
} }

20
flight/Modules/Altitude/altitude.c
View File

@ -131,8 +131,8 @@ static void altitudeTask(void *parameters)
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata; SonarAltitudeData sonardata;
int32_t value=0,timeout=5; int32_t value = 0, timeout = 5;
float coeff=0.25,height_out=0,height_in=0; float coeff = 0.25, height_out = 0, height_in = 0;
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
} }
@ -149,25 +149,23 @@ static void altitudeTask(void *parameters)
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude // Compute the current altitude
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
if(PIOS_HCSR04_Completed()) if(PIOS_HCSR04_Completed()) {
{
value = PIOS_HCSR04_Get(); value = PIOS_HCSR04_Get();
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m //from 3.4cm to 5.1m
{ if((value > 100) && (value < 15000)) {
height_in = value*0.00034f/2.0f; height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff); height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us sonardata.Altitude = height_out; // m/us
} }
// Update the AltitudeActual UAVObject // Update the AltitudeActual UAVObject
SonarAltitudeSet(&sonardata); SonarAltitudeSet(&sonardata);
timeout=5; timeout = 5;
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
} }
if(!(timeout--)) if(!(timeout--)) {
{
//retrigger //retrigger
timeout=5; timeout = 5;
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
} }
} }

25
flight/Modules/Altitude/revolution/altitude.c
View File

@ -90,8 +90,8 @@ static void altitudeTask(void *parameters)
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata; SonarAltitudeData sonardata;
int32_t value=0,timeout=10,sample_rate=0; int32_t value = 0, timeout = 10, sample_rate = 0;
float coeff=0.25,height_out=0,height_in=0; float coeff = 0.25, height_out = 0, height_in = 0;
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
#endif #endif
@ -110,30 +110,27 @@ static void altitudeTask(void *parameters)
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude // Compute the current altitude
// depends on baro samplerate // depends on baro samplerate
if(!(sample_rate--)) if(!(sample_rate--)) {
{ if(PIOS_HCSR04_Completed()) {
if(PIOS_HCSR04_Completed())
{
value = PIOS_HCSR04_Get(); value = PIOS_HCSR04_Get();
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m //from 3.4cm to 5.1m
{ if((value > 100) && (value < 15000)) {
height_in = value*0.00034f/2.0f; height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff); height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us sonardata.Altitude = height_out; // m/us
} }
// Update the SonarAltitude UAVObject // Update the SonarAltitude UAVObject
SonarAltitudeSet(&sonardata); SonarAltitudeSet(&sonardata);
timeout=10; timeout = 10;
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
} }
if(!(timeout--)) if(!(timeout--)) {
{
//retrigger //retrigger
timeout=10; timeout = 10;
PIOS_HCSR04_Trigger(); PIOS_HCSR04_Trigger();
} }
sample_rate=25; sample_rate = 25;
} }
#endif #endif
float temp, press; float temp, press;

1540
flight/Modules/ManualControl/manualcontrol.c
View File

@ -73,11 +73,7 @@
// Private types // Private types
typedef enum typedef enum
{ {
ARM_STATE_DISARMED, ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t; } ArmState_t;
// Private variables // Private variables
@ -114,10 +110,11 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm { struct rcvr_activity_fsm
ManualControlSettingsChannelGroupsOptions group; {
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; ManualControlSettingsChannelGroupsOptions group;
uint8_t sample_count; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
}; };
static struct rcvr_activity_fsm activity_fsm; static struct rcvr_activity_fsm activity_fsm;
@ -131,12 +128,12 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
*/ */
int32_t ManualControlStart() int32_t ManualControlStart()
{ {
// Start main task // Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0; return 0;
} }
/** /**
@ -145,555 +142,552 @@ int32_t ManualControlStart()
int32_t ManualControlInitialize() int32_t ManualControlInitialize()
{ {
/* Check the assumptions about uavobject enum's are correct */ /* Check the assumptions about uavobject enum's are correct */
if(!assumptions) if (!assumptions)
return -1; return -1;
AccessoryDesiredInitialize(); AccessoryDesiredInitialize();
ManualControlCommandInitialize(); ManualControlCommandInitialize();
FlightStatusInitialize(); FlightStatusInitialize();
StabilizationDesiredInitialize(); StabilizationDesiredInitialize();
ReceiverActivityInitialize(); ReceiverActivityInitialize();
ManualControlSettingsInitialize(); ManualControlSettingsInitialize();
return 0; return 0;
} }
MODULE_INITCALL(ManualControlInitialize, ManualControlStart) MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
/** /**
* Module task * Module task
*/ */
static void manualControlTask(void *parameters) static void manualControlTask(void *parameters)
{ {
ManualControlSettingsData settings; ManualControlSettingsData settings;
ManualControlCommandData cmd; ManualControlCommandData cmd;
FlightStatusData flightStatus; FlightStatusData flightStatus;
float flightMode = 0; float flightMode = 0;
uint8_t disconnected_count = 0; uint8_t disconnected_count = 0;
uint8_t connected_count = 0; uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used // this includes not even registering it if not used
AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance();
// Run this initially to make sure the configuration is checked // Run this initially to make sure the configuration is checked
configuration_check(); configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
// Whenever the configuration changes, make sure it is safe to fly
// Make sure unarmed on power up // Whenever the configuration changes, make sure it is safe to fly
ManualControlCommandGet(&cmd); SystemSettingsConnectCallback(configurationUpdatedCb);
FlightStatusGet(&flightStatus); ManualControlSettingsConnectCallback(configurationUpdatedCb);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
/* Initialize the RcvrActivty FSM */ // Whenever the configuration changes, make sure it is safe to fly
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Main task loop // Make sure unarmed on power up
lastSysTime = xTaskGetTickCount(); ManualControlCommandGet(&cmd);
while (1) { FlightStatusGet(&flightStatus);
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM]; flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Wait until next update /* Initialize the RcvrActivty FSM */
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); portTickType lastActivityTime = xTaskGetTickCount();
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); resetRcvrActivity(&activity_fsm);
// Read settings // Main task loop
ManualControlSettingsGet(&settings); lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
/* Update channel activity monitor */ // Wait until next update
if (flightStatus.Armed == ARM_STATE_DISARMED) { vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
if (updateRcvrActivity(&activity_fsm)) { PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) { // Read settings
FlightTelemetryStatsData flightTelemStats; ManualControlSettingsGet(&settings);
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
if (!ManualControlCommandReadOnly()) { /* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
bool valid_input_detected = true; if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
// Read channel values in us FlightTelemetryStatsGet(&flightTelemStats);
for (uint8_t n = 0; if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; /* trying to fly via GCS and lost connection. fall back to transmitter */
++n) { UAVObjMetadata metadata;
extern uint32_t pios_rcvr_group_map[]; ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (!ManualControlCommandReadOnly()) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
settings.ChannelNumber[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
valid_input_detected = false;
else
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm bool valid_input_detected = true;
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) && (
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // Read channel values in us
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
ManualControlCommandSet(&cmd); extern uint32_t pios_rcvr_group_map[];
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
// immediately disarm cmd.Channel[n] = PIOS_RCVR_INVALID;
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]);
}
continue; // If a channel has timed out this is not valid data and we shouldn't update anything
} // until we decide to go to failsafe
if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
valid_input_detected = false;
else
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// decide if we have valid manual input or not // Check settings, if error raise alarm
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
// Implement hysteresis loop on connection status AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
if (valid_input_detected && (++connected_count > 10)) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; ManualControlCommandSet(&cmd);
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
cmd.Throttle = -1; // Shut down engine with no control // immediately disarm
cmd.Roll = 0; setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} else if (valid_input_detected) { continue;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); }
// Scale channels to -1 -> +1 range // decide if we have valid manual input or not
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Apply deadband for Roll/Pitch/Yaw stick inputs // Implement hysteresis loop on connection status
if (settings.Deadband) { if (valid_input_detected && (++connected_count > 10)) {
applyDeadband(&cmd.Roll, settings.Deadband); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
applyDeadband(&cmd.Pitch, settings.Deadband); connected_count = 0;
applyDeadband(&cmd.Yaw, settings.Deadband); disconnected_count = 0;
} } else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms // Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ? float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS; (float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime; lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF #endif // USE_INPUT_LPF
if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID && if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER && && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode); AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} processFlightMode(&settings, flightMode);
// Process arming outside conditional so system will disarm when disconnected }
processArm(&cmd, &settings);
// Process arming outside conditional so system will disarm when disconnected
// Update cmd object processArm(&cmd, &settings);
ManualControlCommandSet(&cmd);
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX) #if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) { if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id, PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel, cmd.Channel,
settings.ChannelMin, settings.ChannelMin,
settings.ChannelMax, settings.ChannelMax,
NELEMENTS(cmd.Channel)); NELEMENTS(cmd.Channel));
} }
#endif /* PIOS_INCLUDE_USB_RCTX */ #endif /* PIOS_INCLUDE_USB_RCTX */
} else { } else {
ManualControlCommandGet(&cmd); /* Under GCS control */ ManualControlCommandGet(&cmd); /* Under GCS control */
} }
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects // Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED: case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture! // This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break; break;
case FLIGHTMODE_MANUAL: case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd); updateActuatorDesired(&cmd);
break; break;
case FLIGHTMODE_STABILIZED: case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings); updateStabilizationDesired(&cmd, &settings);
break; break;
case FLIGHTMODE_TUNING: case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to // Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors. // call anything else. This just avoids errors.
break; break;
case FLIGHTMODE_GUIDANCE: case FLIGHTMODE_GUIDANCE:
switch(flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI: case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors. // No need to call anything. This just avoids errors.
break; break;
case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break; break;
default: default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
} }
break; break;
} }
lastFlightMode = flightStatus.FlightMode; lastFlightMode = flightStatus.FlightMode;
} }
} }
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
{ {
ReceiverActivityData data; ReceiverActivityData data;
bool updated = false; bool updated = false;
/* Clear all channel activity flags */ /* Clear all channel activity flags */
ReceiverActivityGet(&data); ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveChannel != 255) { data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; data.ActiveChannel = 255;
data.ActiveChannel = 255; updated = true;
updated = true; }
} if (updated) {
if (updated) { ReceiverActivitySet(&data);
ReceiverActivitySet(&data); }
}
/* Reset the FSM state */ /* Reset the FSM state */
fsm->group = 0; fsm->group = 0;
fsm->sample_count = 0; fsm->sample_count = 0;
} }
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
for (uint8_t channel = 1; channel <= max_channels; channel++) { {
// Subtract 1 because channels are 1 indexed for (uint8_t channel = 1; channel <= max_channels; channel++) {
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); // Subtract 1 because channels are 1 indexed
} samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
} }
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm)
{ {
bool activity_updated = false; bool activity_updated = false;
/* Compare the current value to the previous sampled value */ /* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; uint16_t delta;
channel++) { uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t delta; uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed if (curr > prev) {
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); delta = curr - prev;
if (curr > prev) { } else {
delta = curr - prev; delta = prev - curr;
} else { }
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */ /* Mark this channel as active */
ReceiverActivityActiveGroupOptions group; ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) { switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break; break;
default: default:
PIOS_Assert(0); PIOS_Assert(0);
break; break;
} }
ReceiverActivityActiveGroupSet((uint8_t*)&group); ReceiverActivityActiveGroupSet((uint8_t*) &group);
ReceiverActivityActiveChannelSet(&channel); ReceiverActivityActiveChannelSet(&channel);
activity_updated = true; activity_updated = true;
} }
} }
return (activity_updated); return (activity_updated);
} }
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
{ {
bool activity_updated = false; bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */ /* We're out of range, reset things */
resetRcvrActivity(fsm); resetRcvrActivity(fsm);
} }
extern uint32_t pios_rcvr_group_map[]; extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) { if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */ /* Unbound group, skip it */
goto group_completed; goto group_completed;
} }
if (fsm->sample_count == 0) { if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */ /* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->prev, fsm->sample_count++;
NELEMENTS(fsm->prev)); return (false);
fsm->sample_count++; }
return (false);
}
/* Compare with previous sample */ /* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed: group_completed:
/* Reset the sample counter */ /* Reset the sample counter */
fsm->sample_count = 0; fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */ /* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */ /* Move to the next group */
fsm->group++; fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */ /* Wrap back to the first group */
fsm->group = 0; fsm->group = 0;
} }
if (pios_rcvr_group_map[fsm->group]) { if (pios_rcvr_group_map[fsm->group]) {
/* /*
* Found an active group, take a sample here to avoid an * Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up * extra 20ms delay in the main thread so we can speed up
* this algorithm. * this algorithm.
*/ */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->prev, fsm->sample_count++;
NELEMENTS(fsm->prev)); break;
fsm->sample_count++; }
break; }
}
}
return (activity_updated); return (activity_updated);
} }
static void updateActuatorDesired(ManualControlCommandData * cmd) static void updateActuatorDesired(ManualControlCommandData * cmd)
{ {
ActuatorDesiredData actuator; ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator); ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll; actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch; actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw; actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator); ActuatorDesiredSet(&actuator);
} }
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{ {
StabilizationDesiredData stabilization; StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization); StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings; uint8_t * stab_settings;
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings; stab_settings = settings->Stabilization1Settings;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings; stab_settings = settings->Stabilization2Settings;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings; stab_settings = settings->Stabilization3Settings;
break; break;
default: default:
// Major error, this should not occur because only enter this block when one of these is true // Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return; return;
} }
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
0; // this is an invalid mode (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
; cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : ;
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
0; // this is an invalid mode (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization); StabilizationDesiredSet(&stabilization);
} }
#if defined(REVOLUTION) #if defined(REVOLUTION)
@ -704,87 +698,86 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
*/ */
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home) static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
{ {
static portTickType lastSysTime; static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
if (home && changed) { if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position // Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0; pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} else if(changed) { } else if(changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else { } else {
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
*/ */
} }
} }
static void updateLandDesired(ManualControlCommandData * cmd, bool changed) static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
{ {
static portTickType lastSysTime; static portTickType lastSysTime;
portTickType thisSysTime; portTickType thisSysTime;
float dT; float dT;
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
if(changed) { if(changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
} }
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
/** /**
@ -794,41 +787,43 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
*/ */
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{ {
const float DEADBAND_HIGH = 0.55; const float DEADBAND_HIGH = 0.55;
const float DEADBAND_LOW = 0.45; const float DEADBAND_LOW = 0.45;
static portTickType lastSysTime;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
StabilizationSettingsData stabSettings; static portTickType lastSysTime;
StabilizationSettingsGet(&stabSettings); static bool zeroed = false;
portTickType thisSysTime;
float dT;
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
thisSysTime = xTaskGetTickCount(); StabilizationSettingsData stabSettings;
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; StabilizationSettingsGet(&stabSettings);
lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; thisSysTime = xTaskGetTickCount();
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; lastSysTime = thisSysTime;
float currentDown; altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
PositionActualDownGet(&currentDown); altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
if(changed) { altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
// After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0; float currentDown;
zeroed = false; PositionActualDownGet(&currentDown);
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) if(changed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; // After not being in this mode for a while init at current height
else if (cmd->Throttle < DEADBAND_LOW && zeroed) altitudeHoldDesired.Altitude = 0;
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT; zeroed = false;
else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) // Require the stick to enter the dead band before they can move height } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
zeroed = true; altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
AltitudeHoldDesiredSet(&altitudeHoldDesired); altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height
zeroed = true;
}
AltitudeHoldDesiredSet(&altitudeHoldDesired);
} }
#else #else
@ -837,17 +832,17 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
// in flight // in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home) static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
static void updateLandDesired(ManualControlCommandData * cmd, bool changed) static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
#endif #endif
/** /**
@ -855,36 +850,39 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
*/ */
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{ {
float valueScaled; float valueScaled;
// Scale // Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
{ if (max != neutral) {
if (max != neutral) valueScaled = (float) (value - neutral) / (float) (max - neutral);
valueScaled = (float)(value - neutral) / (float)(max - neutral); } else {
else valueScaled = 0;
valueScaled = 0; }
} } else {
else if (min != neutral) {
{ valueScaled = (float) (value - neutral) / (float) (neutral - min);
if (min != neutral) } else {
valueScaled = (float)(value - neutral) / (float)(neutral - min); valueScaled = 0;
else }
valueScaled = 0; }
}
// Bound // Bound
if (valueScaled > 1.0) valueScaled = 1.0; if (valueScaled > 1.0) {
else valueScaled = 1.0;
if (valueScaled < -1.0) valueScaled = -1.0; } else if (valueScaled < -1.0) {
valueScaled = -1.0;
}
return valueScaled; return valueScaled;
} }
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
if(end_time > start_time) {
return (end_time - start_time) * portTICK_RATE_MS; if (end_time > start_time) {
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; return (end_time - start_time) * portTICK_RATE_MS;
}
return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS;
} }
/** /**
@ -893,24 +891,21 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
*/ */
static bool okToArm(void) static bool okToArm(void)
{ {
// read alarms // read alarms
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
// Check each alarm return false;
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) }
{ }
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false; return true;
}
}
return true;
} }
/** /**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm * @brief Determine if the aircraft is forced to disarm by an explicit alarm
@ -918,28 +913,29 @@ static bool okToArm(void)
*/ */
static bool forcedDisArm(void) static bool forcedDisArm(void)
{ {
// read alarms // read alarms
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
return true; return true;
} }
return false; return false;
} }
/** /**
* @brief Update the flightStatus object only if value changed. Reduces callbacks * @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value * @param[in] val The new value
*/ */
static void setArmedIfChanged(uint8_t val) { static void setArmedIfChanged(uint8_t val)
FlightStatusData flightStatus; {
FlightStatusGet(&flightStatus); FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) { if (flightStatus.Armed != val) {
flightStatus.Armed = val; flightStatus.Armed = val;
FlightStatusSet(&flightStatus); FlightStatusSet(&flightStatus);
} }
} }
/** /**
@ -950,116 +946,121 @@ static void setArmedIfChanged(uint8_t val) {
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{ {
bool lowThrottle = cmd->Throttle <= 0; bool lowThrottle = cmd->Throttle <= 0;
if (forcedDisArm()) { if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return; return;
} }
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm // In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else { } else {
// Not really needed since this function not called when disconnected // Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
lowThrottle = true; lowThrottle = true;
// The throttle is not low, in case we where arming or disarming, abort // The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) { if (!lowThrottle) {
switch(armState) { switch (armState) {
case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT: case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED; armState = ARM_STATE_ARMED;
break; break;
case ARM_STATE_ARMING_MANUAL: case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED; armState = ARM_STATE_DISARMED;
break; break;
default: default:
// Nothing needs to be done in the other states // Nothing needs to be done in the other states
break; break;
} }
return; return;
} }
// The rest of these cases throttle is low // The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm // In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return; return;
} }
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// When the configuration is not "Always armed" and no "Always disarmed", // Calc channel see assumptions7
// the state will not be changed when the throttle is not low int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
static portTickType armedDisarmStart; switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
float armingInputLevel = 0; case ARMING_CHANNEL_ROLL:
armingInputLevel = sign * cmd->Roll;
break;
case ARMING_CHANNEL_PITCH:
armingInputLevel = sign * cmd->Pitch;
break;
case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw;
break;
}
// Calc channel see assumptions7 bool manualArm = false;
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; bool manualDisarm = false;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false; if (armingInputLevel <= -ARMED_THRESHOLD)
bool manualDisarm = false; manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
if (armingInputLevel <= -ARMED_THRESHOLD) switch (armState) {
manualArm = true; case ARM_STATE_DISARMED:
else if (armingInputLevel >= +ARMED_THRESHOLD) setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
manualDisarm = true;
switch(armState) { // only allow arming if it's OK too
case ARM_STATE_DISARMED: if (manualArm && okToArm()) {
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
// only allow arming if it's OK too case ARM_STATE_ARMING_MANUAL:
if (manualArm && okToArm()) { setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL: if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) case ARM_STATE_ARMED:
armState = ARM_STATE_ARMED; // When we get here, the throttle is low,
else if (!manualArm) // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armState = ARM_STATE_DISARMED; armedDisarmStart = lastSysTime;
break; armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_ARMED: case ARM_STATE_DISARMING_TIMEOUT:
// When we get here, the throttle is low, // We get here when armed while throttle low, even when the arming timeout is not enabled
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMED;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT: // Switch to disarming due to manual control when needed
// We get here when armed while throttle low, even when the arming timeout is not enabled if (manualDisarm) {
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMED; armState = ARM_STATE_DISARMING_MANUAL;
}
break;
// Switch to disarming due to manual control when needed case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm) { if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMED;
armState = ARM_STATE_DISARMING_MANUAL; else if (!manualDisarm)
} armState = ARM_STATE_ARMED;
break; break;
} // End Switch
case ARM_STATE_DISARMING_MANUAL: }
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
} }
/** /**
@ -1070,20 +1071,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
*/ */
static void processFlightMode(ManualControlSettingsData *settings, float flightMode) static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
{ {
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
// Convert flightMode value into the switch position in the range [0..N-1] // Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber) if (pos >= settings->FlightModeNumber)
pos = settings->FlightModeNumber - 1; pos = settings->FlightModeNumber - 1;
uint8_t newMode = settings->FlightModePosition[pos]; uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) { if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode; flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus); FlightStatusSet(&flightStatus);
} }
} }
/** /**
@ -1092,13 +1093,12 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
*/ */
bool validInputRange(int16_t min, int16_t max, uint16_t value) bool validInputRange(int16_t min, int16_t max, uint16_t value)
{ {
if (min > max) if (min > max) {
{ int16_t tmp = min;
int16_t tmp = min; min = max;
min = max; max = tmp;
max = tmp; }
} return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
} }
/** /**
@ -1106,13 +1106,12 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
*/ */
static void applyDeadband(float *value, float deadband) static void applyDeadband(float *value, float deadband)
{ {
if (fabs(*value) < deadband) if (fabs(*value) < deadband)
*value = 0.0f; *value = 0.0f;
else else if (*value > 0.0f)
if (*value > 0.0f) *value -= deadband;
*value -= deadband; else
else *value += deadband;
*value += deadband;
} }
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
@ -1121,25 +1120,22 @@ static void applyDeadband(float *value, float deadband)
*/ */
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{ {
if (settings->ResponseTime[channel]) { if (settings->ResponseTime[channel]) {
float rt = (float)settings->ResponseTime[channel]; float rt = (float)settings->ResponseTime[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel]; *value = inputFiltered[channel];
} }
} }
#endif // USE_INPUT_LPF #endif // USE_INPUT_LPF
/** /**
* Called whenever a critical configuration component changes * Called whenever a critical configuration component changes
*/ */
static void configurationUpdatedCb(UAVObjEvent * ev) static void configurationUpdatedCb(UAVObjEvent * ev)
{ {
configuration_check(); configuration_check();
} }
/** /**
* @} * @}
* @} * @}
*/ */

3322
flight/Modules/Osd/osdgen/osdgen.c
View File

@ -29,7 +29,6 @@
*/ */
// **************** // ****************
#include "openpilot.h" #include "openpilot.h"
#include "osdgen.h" #include "osdgen.h"
#include "attitudeactual.h" #include "attitudeactual.h"
@ -47,29 +46,28 @@
#include "splash.h" #include "splash.h"
/* /*
static uint16_t angleA=0; static uint16_t angleA=0;
static int16_t angleB=90; static int16_t angleB=90;
static int16_t angleC=0; static int16_t angleC=0;
static int16_t sum=2; static int16_t sum=2;
static int16_t m_pitch=0; static int16_t m_pitch=0;
static int16_t m_roll=0; static int16_t m_roll=0;
static int16_t m_yaw=0; static int16_t m_yaw=0;
static int16_t m_batt=0; static int16_t m_batt=0;
static int16_t m_alt=0; static int16_t m_alt=0;
static uint8_t m_gpsStatus=0; static uint8_t m_gpsStatus=0;
static int32_t m_gpsLat=0; static int32_t m_gpsLat=0;
static int32_t m_gpsLon=0; static int32_t m_gpsLon=0;
static float m_gpsAlt=0; static float m_gpsAlt=0;
static float m_gpsSpd=0;*/ static float m_gpsSpd=0;*/
extern uint8_t *draw_buffer_level; extern uint8_t *draw_buffer_level;
extern uint8_t *draw_buffer_mask; extern uint8_t *draw_buffer_mask;
extern uint8_t *disp_buffer_level; extern uint8_t *disp_buffer_level;
extern uint8_t *disp_buffer_mask; extern uint8_t *disp_buffer_mask;
TTime timex; TTime timex;
// **************** // ****************
@ -94,143 +92,136 @@ static xTaskHandle osdgenTaskHandle;
struct splashEntry struct splashEntry
{ {
unsigned int width, height; unsigned int width, height;
const uint16_t *level; const uint16_t *level;
const uint16_t *mask; const uint16_t *mask;
}; };
struct splashEntry splash[3] = { struct splashEntry splash[3] =
{ oplogo_width, {
oplogo_height, { oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits },
oplogo_bits, { level_width, level_height, level_bits, level_mask_bits },
oplogo_mask_bits }, { llama_width, llama_height, llama_bits, llama_mask_bits },
{ level_width,
level_height,
level_bits,
level_mask_bits },
{ llama_width,
llama_height,
llama_bits,
llama_mask_bits },
}; };
uint16_t mirror(uint16_t source) uint16_t mirror(uint16_t source)
{ {
int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1)
((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5)
((source & 0x0800) << 1) | ((source & 0x0400) << 3) | | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5)
((source & 0x0200) << 5) | ((source & 0x0100) << 7) | | ((source & 0x0001) << 7);
((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) |
((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) |
((source & 0x0008) << 1) | ((source & 0x0004) << 3) |
((source & 0x0002) << 5) | ((source & 0x0001) << 7);
return result; return result;
} }
void clearGraphics() { void clearGraphics()
memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); {
memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
} }
void copyimage(uint16_t offsetx, uint16_t offsety, int image) { void copyimage(uint16_t offsetx, uint16_t offsety, int image)
//check top/left position {
if (!validPos(offsetx, offsety)) { //check top/left position
return; if (!validPos(offsetx, offsety)) {
} return;
struct splashEntry splash_info; }
splash_info = splash[image]; struct splashEntry splash_info;
offsetx=offsetx/8; splash_info = splash[image];
for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) { offsetx = offsetx / 8;
uint16_t x1=offsetx; for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) { uint16_t x1 = offsetx;
draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8); for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF); draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
draw_buffer_mask[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8); mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF); draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
x1+=2; mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
} draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
} mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
x1 += 2;
}
}
} }
uint8_t validPos(uint16_t x, uint16_t y) { uint8_t validPos(uint16_t x, uint16_t y)
if ( x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) { {
return 0; if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
} return 0;
return 1; }
return 1;
} }
// Credit for this one goes to wikipedia! :-) // Credit for this one goes to wikipedia! :-)
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) { void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius)
int f = 1 - radius; {
int ddF_x = 1; int f = 1 - radius;
int ddF_y = -2 * radius; int ddF_x = 1;
int x = 0; int ddF_y = -2 * radius;
int y = radius; int x = 0;
int y = radius;
write_pixel_lm(x0, y0 + radius,1,1); write_pixel_lm(x0, y0 + radius, 1, 1);
write_pixel_lm(x0, y0 - radius,1,1); write_pixel_lm(x0, y0 - radius, 1, 1);
write_pixel_lm(x0 + radius, y0,1,1); write_pixel_lm(x0 + radius, y0, 1, 1);
write_pixel_lm(x0 - radius, y0,1,1); write_pixel_lm(x0 - radius, y0, 1, 1);
while(x < y) while (x < y) {
{ // ddF_x == 2 * x + 1;
// ddF_x == 2 * x + 1; // ddF_y == -2 * y;
// ddF_y == -2 * y; // f == x*x + y*y - radius*radius + 2*x - y + 1;
// f == x*x + y*y - radius*radius + 2*x - y + 1; if (f >= 0) {
if(f >= 0) y--;
{ ddF_y += 2;
y--; f += ddF_y;
ddF_y += 2; }
f += ddF_y; x++;
} ddF_x += 2;
x++; f += ddF_x;
ddF_x += 2; write_pixel_lm(x0 + x, y0 + y, 1, 1);
f += ddF_x; write_pixel_lm(x0 - x, y0 + y, 1, 1);
write_pixel_lm(x0 + x, y0 + y,1,1); write_pixel_lm(x0 + x, y0 - y, 1, 1);
write_pixel_lm(x0 - x, y0 + y,1,1); write_pixel_lm(x0 - x, y0 - y, 1, 1);
write_pixel_lm(x0 + x, y0 - y,1,1); write_pixel_lm(x0 + y, y0 + x, 1, 1);
write_pixel_lm(x0 - x, y0 - y,1,1); write_pixel_lm(x0 - y, y0 + x, 1, 1);
write_pixel_lm(x0 + y, y0 + x,1,1); write_pixel_lm(x0 + y, y0 - x, 1, 1);
write_pixel_lm(x0 - y, y0 + x,1,1); write_pixel_lm(x0 - y, y0 - x, 1, 1);
write_pixel_lm(x0 + y, y0 - x,1,1); }
write_pixel_lm(x0 - y, y0 - x,1,1);
}
} }
void swap(uint16_t* a, uint16_t* b) { void swap(uint16_t* a, uint16_t* b)
uint16_t temp = *a; {
*a = *b; uint16_t temp = *a;
*b = temp; *a = *b;
*b = temp;
} }
const static int8_t sinData[91] =
{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };
const static int8_t sinData[91] = { static int8_t mySin(uint16_t angle)
0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, {
34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, uint16_t pos = 0;
63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, pos = angle % 360;
85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, int8_t mult = 1;
97, 98, 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100}; // 180-359 is same as 0-179 but negative.
if (pos >= 180) {
static int8_t mySin(uint16_t angle) { pos = pos - 180;
uint16_t pos = 0; mult = -1;
pos = angle % 360; }
int8_t mult = 1; // 0-89 is equal to 90-179 except backwards.
// 180-359 is same as 0-179 but negative. if (pos >= 90) {
if (pos >= 180) { pos = 180 - pos;
pos = pos - 180; }
mult = -1; return mult * (int8_t)(sinData[pos]);
}
// 0-89 is equal to 90-179 except backwards.
if (pos >= 90) {
pos = 180 - pos;
}
return mult * (int8_t)(sinData[pos]);
} }
static int8_t myCos(uint16_t angle) { static int8_t myCos(uint16_t angle)
return mySin(angle + 90); {
return mySin(angle + 90);
} }
/// Draws four points relative to the given center point. /// Draws four points relative to the given center point.
@ -247,10 +238,10 @@ static int8_t myCos(uint16_t angle) {
/// \param color the color to draw the pixels with. /// \param color the color to draw the pixels with.
void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY) void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY)
{ {
write_pixel_lm(centerX + deltaX, centerY + deltaY,1,1); // Ist Quadrant write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant
write_pixel_lm(centerX - deltaX, centerY + deltaY,1,1); // IInd Quadrant write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant
write_pixel_lm(centerX - deltaX, centerY - deltaY,1,1); // IIIrd Quadrant write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant
write_pixel_lm(centerX + deltaX, centerY - deltaY,1,1); // IVth Quadrant write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant
} }
/// Implements the midpoint ellipse drawing algorithm which is a bresenham /// Implements the midpoint ellipse drawing algorithm which is a bresenham
@ -275,62 +266,58 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
plotFourQuadrants(centerX, centerY, x, y); plotFourQuadrants(centerX, centerY, x, y);
while(deltaY >= deltaX) while (deltaY >= deltaX) {
{ x++;
x++; deltaX += (doubleVerticalRadius << 1);
deltaX += (doubleVerticalRadius << 1);
error += deltaX + doubleVerticalRadius; error += deltaX + doubleVerticalRadius;
if(error >= 0) if (error >= 0) {
{ y--;
y--; deltaY -= (doubleHorizontalRadius << 1);
deltaY -= (doubleHorizontalRadius << 1);
error -= deltaY; error -= deltaY;
} }
plotFourQuadrants(centerX, centerY, x, y); plotFourQuadrants(centerX, centerY, x, y);
} }
error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); error = (int64_t)(
doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
while (y>=0) while (y >= 0) {
{ error += doubleHorizontalRadius;
error += doubleHorizontalRadius; y--;
y--; deltaY -= (doubleHorizontalRadius << 1);
deltaY -= (doubleHorizontalRadius<<1); error -= deltaY;
error -= deltaY;
if(error <= 0) if (error <= 0) {
{ x++;
x++; deltaX += (doubleVerticalRadius << 1);
deltaX += (doubleVerticalRadius << 1); error += deltaX;
error += deltaX; }
}
plotFourQuadrants(centerX, centerY, x, y); plotFourQuadrants(centerX, centerY, x, y);
} }
} }
void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size) void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size)
{ {
int16_t a = myCos(angle); int16_t a = myCos(angle);
int16_t b = mySin(angle); int16_t b = mySin(angle);
a = (a * (size/2)) / 100; a = (a * (size / 2)) / 100;
b = (b * (size/2)) / 100; b = (b * (size / 2)) / 100;
write_line_lm((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a, 1, 1); //Direction line write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line
//write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 - a/2, (y)-1 - b/2, 1, 1); // Arrow "wings" write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings"
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 + a/2, (y)-1 + b/2, 1, 1); write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1);
} }
void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{ {
write_line_lm(x1, y1, x2, y1, 1, 1); //top write_line_lm(x1, y1, x2, y1, 1, 1); //top
write_line_lm(x1, y1, x1, y2, 1, 1); //left write_line_lm(x1, y1, x1, y2, 1, 1); //left
write_line_lm(x2, y1, x2, y2, 1, 1); //right write_line_lm(x2, y1, x2, y2, 1, 1); //right
write_line_lm(x1, y2, x2, y2, 1, 1); //bottom write_line_lm(x1, y2, x2, y2, 1, 1); //bottom
} }
// simple routines // simple routines
@ -347,14 +334,14 @@ void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
*/ */
void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode) void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
{ {
CHECK_COORDS(x, y); CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word // Determine the bit in the word to be set and the word
// index to set it in. // index to set it in.
int bitnum = CALC_BIT_IN_WORD(x); int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y); int wordnum = CALC_BUFF_ADDR(x, y);
// Apply a mask. // Apply a mask.
uint16_t mask = 1 << (7 - bitnum); uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(buff, wordnum, mask, mode); WRITE_WORD_MODE(buff, wordnum, mask, mode);
} }
/** /**
@ -368,18 +355,17 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
*/ */
void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode) void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
{ {
CHECK_COORDS(x, y); CHECK_COORDS(x, y);
// Determine the bit in the word to be set and the word // Determine the bit in the word to be set and the word
// index to set it in. // index to set it in.
int bitnum = CALC_BIT_IN_WORD(x); int bitnum = CALC_BIT_IN_WORD(x);
int wordnum = CALC_BUFF_ADDR(x, y); int wordnum = CALC_BUFF_ADDR(x, y);
// Apply the masks. // Apply the masks.
uint16_t mask = 1 << (7 - bitnum); uint16_t mask = 1 << (7 - bitnum);
WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode); WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode); WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
} }
/** /**
* write_hline: optimised horizontal line writing algorithm * write_hline: optimised horizontal line writing algorithm
* *
@ -391,41 +377,38 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
*/ */
void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode) void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode)
{ {
CLIP_COORDS(x0, y); CLIP_COORDS(x0, y);
CLIP_COORDS(x1, y); CLIP_COORDS(x1, y);
if(x0 > x1) if (x0 > x1) {
{ SWAP(x0, x1);
SWAP(x0, x1); }
} if (x0 == x1) {
if(x0 == x1) return; return;
/* This is an optimised algorithm for writing horizontal lines. }
* We begin by finding the addresses of the x0 and x1 points. */ /* This is an optimised algorithm for writing horizontal lines.
int addr0 = CALC_BUFF_ADDR(x0, y); * We begin by finding the addresses of the x0 and x1 points. */
int addr1 = CALC_BUFF_ADDR(x1, y); int addr0 = CALC_BUFF_ADDR(x0, y);
int addr0_bit = CALC_BIT_IN_WORD(x0); int addr1 = CALC_BUFF_ADDR(x1, y);
int addr1_bit = CALC_BIT_IN_WORD(x1); int addr0_bit = CALC_BIT_IN_WORD(x0);
int mask, mask_l, mask_r, i; int addr1_bit = CALC_BIT_IN_WORD(x1);
/* If the addresses are equal, we only need to write one word int mask, mask_l, mask_r, i;
* which is an island. */ /* If the addresses are equal, we only need to write one word
if(addr0 == addr1) * which is an island. */
{ if (addr0 == addr1) {
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask, mode); WRITE_WORD_MODE(buff, addr0, mask, mode);
} } else {
/* Otherwise we need to write the edges and then the middle. */ /* Otherwise we need to write the edges and then the middle. */
else mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
{ mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); WRITE_WORD_MODE(buff, addr0, mask_l, mode);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); WRITE_WORD_MODE(buff, addr1, mask_r, mode);
WRITE_WORD_MODE(buff, addr0, mask_l, mode); // Now write 0xffff words from start+1 to end-1.
WRITE_WORD_MODE(buff, addr1, mask_r, mode); for (i = addr0 + 1; i <= addr1 - 1; i++) {
// Now write 0xffff words from start+1 to end-1. uint8_t m = 0xff;
for(i = addr0 + 1; i <= addr1 - 1; i++) WRITE_WORD_MODE(buff, i, m, mode);
{ }
uint8_t m=0xff; }
WRITE_WORD_MODE(buff, i, m, mode);
}
}
} }
/** /**
@ -439,10 +422,10 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
*/ */
void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode) void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode)
{ {
// TODO: an optimisation would compute the masks and apply to // TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously. // both buffers simultaneously.
write_hline(draw_buffer_level, x0, x1, y, lmode); write_hline(draw_buffer_level, x0, x1, y, lmode);
write_hline(draw_buffer_mask, x0, x1, y, mmode); write_hline(draw_buffer_mask, x0, x1, y, mmode);
} }
/** /**
@ -459,19 +442,18 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode,
*/ */
void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode) void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode)
{ {
int stroke, fill; int stroke, fill;
SETUP_STROKE_FILL(stroke, fill, mode) SETUP_STROKE_FILL(stroke, fill, mode)
if(x0 > x1) if (x0 > x1) {
{ SWAP(x0, x1);
SWAP(x0, x1); }
} // Draw the main body of the line.
// Draw the main body of the line. write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode); write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode); write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode); // Draw the endcaps, if any.
// Draw the endcaps, if any. DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode); DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
} }
/** /**
@ -485,27 +467,27 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
*/ */
void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode) void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode)
{ {
unsigned int a; unsigned int a;
CLIP_COORDS(x, y0); CLIP_COORDS(x, y0);
CLIP_COORDS(x, y1); CLIP_COORDS(x, y1);
if(y0 > y1) if (y0 > y1) {
{ SWAP(y0, y1);
SWAP(y0, y1); }
} if (y0 == y1) {
if(y0 == y1) return; return;
/* This is an optimised algorithm for writing vertical lines. }
* We begin by finding the addresses of the x,y0 and x,y1 points. */ /* This is an optimised algorithm for writing vertical lines.
int addr0 = CALC_BUFF_ADDR(x, y0); * We begin by finding the addresses of the x,y0 and x,y1 points. */
int addr1 = CALC_BUFF_ADDR(x, y1); int addr0 = CALC_BUFF_ADDR(x, y0);
/* Then we calculate the pixel data to be written. */ int addr1 = CALC_BUFF_ADDR(x, y1);
int bitnum = CALC_BIT_IN_WORD(x); /* Then we calculate the pixel data to be written. */
uint16_t mask = 1 << (7 - bitnum); int bitnum = CALC_BIT_IN_WORD(x);
/* Run from addr0 to addr1 placing pixels. Increment by the number uint16_t mask = 1 << (7 - bitnum);
* of words n each graphics line. */ /* Run from addr0 to addr1 placing pixels. Increment by the number
for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) * of words n each graphics line. */
{ for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
WRITE_WORD_MODE(buff, a, mask, mode); WRITE_WORD_MODE(buff, a, mask, mode);
} }
} }
/** /**
@ -519,10 +501,10 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
*/ */
void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode) void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode)
{ {
// TODO: an optimisation would compute the masks and apply to // TODO: an optimisation would compute the masks and apply to
// both buffers simultaneously. // both buffers simultaneously.
write_vline(draw_buffer_level, x, y0, y1, lmode); write_vline(draw_buffer_level, x, y0, y1, lmode);
write_vline(draw_buffer_mask, x, y0, y1, mmode); write_vline(draw_buffer_mask, x, y0, y1, mmode);
} }
/** /**
@ -539,19 +521,18 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode,
*/ */
void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
{ {
int stroke, fill; int stroke, fill;
if(y0 > y1) if (y0 > y1) {
{ SWAP(y0, y1);
SWAP(y0, y1); }
} SETUP_STROKE_FILL(stroke, fill, mode);
SETUP_STROKE_FILL(stroke, fill, mode); // Draw the main body of the line.
// Draw the main body of the line. write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode); write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode); write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode); // Draw the endcaps, if any.
// Draw the endcaps, if any. DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode); DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
} }
/** /**
@ -570,61 +551,56 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int
*/ */
void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode) void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode)
{ {
int yy, addr0_old, addr1_old; int yy, addr0_old, addr1_old;
CHECK_COORDS(x, y); CHECK_COORDS(x, y);
CHECK_COORD_X(x + width); CHECK_COORD_X(x + width);
CHECK_COORD_Y(y + height); CHECK_COORD_Y(y + height);
if(width <= 0 || height <= 0) return; if (width <= 0 || height <= 0) {
// Calculate as if the rectangle was only a horizontal line. We then return;
// step these addresses through each row until we iterate `height` times. }
int addr0 = CALC_BUFF_ADDR(x, y); // Calculate as if the rectangle was only a horizontal line. We then
int addr1 = CALC_BUFF_ADDR(x + width, y); // step these addresses through each row until we iterate `height` times.
int addr0_bit = CALC_BIT_IN_WORD(x); int addr0 = CALC_BUFF_ADDR(x, y);
int addr1_bit = CALC_BIT_IN_WORD(x + width); int addr1 = CALC_BUFF_ADDR(x + width, y);
int mask, mask_l, mask_r, i; int addr0_bit = CALC_BIT_IN_WORD(x);
// If the addresses are equal, we need to write one word vertically. int addr1_bit = CALC_BIT_IN_WORD(x + width);
if(addr0 == addr1) int mask, mask_l, mask_r, i;
{ // If the addresses are equal, we need to write one word vertically.
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); if (addr0 == addr1) {
while(height--) mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
{ while (height--) {
WRITE_WORD_MODE(buff, addr0, mask, mode); WRITE_WORD_MODE(buff, addr0, mask, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8; addr0 += GRAPHICS_WIDTH_REAL / 8;
}
} }
} else {
// Otherwise we need to write the edges and then the middle repeatedly. // Otherwise we need to write the edges and then the middle repeatedly.
else mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
{ mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); // Write edges first.
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); yy = 0;
// Write edges first. addr0_old = addr0;
yy = 0; addr1_old = addr1;
addr0_old = addr0; while (yy < height) {
addr1_old = addr1; WRITE_WORD_MODE(buff, addr0, mask_l, mode);
while(yy < height) WRITE_WORD_MODE(buff, addr1, mask_r, mode);
{ addr0 += GRAPHICS_WIDTH_REAL / 8;
WRITE_WORD_MODE(buff, addr0, mask_l, mode); addr1 += GRAPHICS_WIDTH_REAL / 8;
WRITE_WORD_MODE(buff, addr1, mask_r, mode); yy++;
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
// Now write 0xffff words from start+1 to end-1 for each row.
yy = 0;
addr0 = addr0_old;
addr1 = addr1_old;
while(yy < height)
{
for(i = addr0 + 1; i <= addr1 - 1; i++)
{
uint8_t m=0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
} }
// Now write 0xffff words from start+1 to end-1 for each row.
yy = 0;
addr0 = addr0_old;
addr1 = addr1_old;
while (yy < height) {
for (i = addr0 + 1; i <= addr1 - 1; i++) {
uint8_t m = 0xff;
WRITE_WORD_MODE(buff, i, m, mode);
}
addr0 += GRAPHICS_WIDTH_REAL / 8;
addr1 += GRAPHICS_WIDTH_REAL / 8;
yy++;
}
}
} }
/** /**
@ -639,8 +615,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
*/ */
void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode) void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode)
{ {
write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode); write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode); write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
} }
/** /**
@ -656,14 +632,14 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt
*/ */
void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode) void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode)
{ {
//CHECK_COORDS(x, y); //CHECK_COORDS(x, y);
//CHECK_COORDS(x + width, y + height); //CHECK_COORDS(x + width, y + height);
//if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
//if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
} }
/** /**
@ -679,22 +655,19 @@ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int hei
*/ */
void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int mode) void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int mode)
{ {
CHECK_COORDS(cx, cy); CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0; int error = -r, x = r, y = 0;
while(x >= y) while (x >= y) {
{ if (dashp == 0 || (y % dashp) < (dashp / 2)) {
if(dashp == 0 || (y % dashp) < (dashp / 2)) CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
{ }
CIRCLE_PLOT_8(buff, cx, cy, x, y, mode); error += (y * 2) + 1;
} y++;
error += (y * 2) + 1; if (error >= 0) {
y++; --x;
if(error >= 0) error -= x * 2;
{ }
--x; }
error -= x * 2;
}
}
} }
/** /**
@ -710,58 +683,51 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
*/ */
void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode) void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode)
{ {
int stroke, fill; int stroke, fill;
CHECK_COORDS(cx, cy); CHECK_COORDS(cx, cy);
SETUP_STROKE_FILL(stroke, fill, mode); SETUP_STROKE_FILL(stroke, fill, mode);
// This is a two step procedure. First, we draw the outline of the // This is a two step procedure. First, we draw the outline of the
// circle, then we draw the inner part. // circle, then we draw the inner part.
int error = -r, x = r, y = 0; int error = -r, x = r, y = 0;
while(x >= y) while (x >= y) {
{ if (dashp == 0 || (y % dashp) < (dashp / 2)) {
if(dashp == 0 || (y % dashp) < (dashp / 2)) CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
{ CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode); if (bmode == 1) {
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
if(bmode == 1) CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
{ CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke); }
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode); }
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke); error += (y * 2) + 1;
} y++;
} if (error >= 0) {
error += (y * 2) + 1; --x;
y++; error -= x * 2;
if(error >= 0) }
{ }
--x; error = -r;
error -= x * 2; x = r;
} y = 0;
} while (x >= y) {
error = -r; if (dashp == 0 || (y % dashp) < (dashp / 2)) {
x = r; CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
y = 0; CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
while(x >= y) }
{ error += (y * 2) + 1;
if(dashp == 0 || (y % dashp) < (dashp / 2)) y++;
{ if (error >= 0) {
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode); --x;
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill); error -= x * 2;
} }
error += (y * 2) + 1; }
y++;
if(error >= 0)
{
--x;
error -= x * 2;
}
}
} }
/** /**
@ -775,42 +741,37 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
*/ */
void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, int mode) void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, int mode)
{ {
CHECK_COORDS(cx, cy); CHECK_COORDS(cx, cy);
int error = -r, x = r, y = 0, xch = 0; int error = -r, x = r, y = 0, xch = 0;
// It turns out that filled circles can take advantage of the midpoint // It turns out that filled circles can take advantage of the midpoint
// circle algorithm. We simply draw very fast horizontal lines across each // circle algorithm. We simply draw very fast horizontal lines across each
// pair of X,Y coordinates. In some cases, this can even be faster than // pair of X,Y coordinates. In some cases, this can even be faster than
// drawing an outlined circle! // drawing an outlined circle!
// //
// Due to multiple writes to each set of pixels, we have a special exception // Due to multiple writes to each set of pixels, we have a special exception
// for when using the toggling draw mode. // for when using the toggling draw mode.
while(x >= y) while (x >= y) {
{ if (y != 0) {
if(y != 0) write_hline(buff, cx - x, cx + x, cy + y, mode);
{ write_hline(buff, cx - x, cx + x, cy - y, mode);
write_hline(buff, cx - x, cx + x, cy + y, mode); if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
write_hline(buff, cx - x, cx + x, cy - y, mode); write_hline(buff, cx - y, cx + y, cy + x, mode);
if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) write_hline(buff, cx - y, cx + y, cy - x, mode);
{ xch = 0;
write_hline(buff, cx - y, cx + y, cy + x, mode); }
write_hline(buff, cx - y, cx + y, cy - x, mode); }
xch = 0; error += (y * 2) + 1;
} y++;
} if (error >= 0) {
error += (y * 2) + 1; --x;
y++; xch = 1;
if(error >= 0) error -= x * 2;
{ }
--x; }
xch = 1; // Handle toggle mode.
error -= x * 2; if (mode == 2) {
} write_hline(buff, cx - r, cx + r, cy, mode);
} }
// Handle toggle mode.
if(mode == 2)
{
write_hline(buff, cx - r, cx + r, cy, mode);
}
} }
/** /**
@ -825,45 +786,39 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
*/ */
void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode) void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode)
{ {
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
int steep = abs(y1 - y0) > abs(x1 - x0); int steep = abs(y1 - y0) > abs(x1 - x0);
if(steep) if (steep) {
{ SWAP(x0, y0);
SWAP(x0, y0); SWAP(x1, y1);
SWAP(x1, y1); }
} if (x0 > x1) {
if(x0 > x1) SWAP(x0, x1);
{ SWAP(y0, y1);
SWAP(x0, x1); }
SWAP(y0, y1); int deltax = x1 - x0;
} int deltay = abs(y1 - y0);
int deltax = x1 - x0; int error = deltax / 2;
int deltay = abs(y1 - y0); int ystep;
int error = deltax / 2; int y = y0;
int ystep; int x; //, lasty = y, stox = 0;
int y = y0; if (y0 < y1) {
int x; //, lasty = y, stox = 0; ystep = 1;
if(y0 < y1) } else {
ystep = 1; ystep = -1;
else }
ystep = -1; for (x = x0; x < x1; x++) {
for(x = x0; x < x1; x++) if (steep) {
{ write_pixel(buff, y, x, mode);
if(steep) } else {
{ write_pixel(buff, x, y, mode);
write_pixel(buff, y, x, mode); }
} error -= deltay;
else if (error < 0) {
{ y += ystep;
write_pixel(buff, x, y, mode); error += deltax;
} }
error -= deltay; }
if(error < 0)
{
y += ystep;
error += deltax;
}
}
} }
/** /**
@ -878,8 +833,8 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
*/ */
void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode) void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode)
{ {
write_line(draw_buffer_mask, x0, y0, x1, y1, mmode); write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
write_line(draw_buffer_level, x0, y0, x1, y1, lmode); write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
} }
/** /**
@ -897,84 +852,70 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i
*/ */
void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
{ {
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
// This could be improved for speed. // This could be improved for speed.
int omode, imode; int omode, imode;
if(mode == 0) if (mode == 0) {
{ omode = 0;
omode = 0; imode = 1;
imode = 1; } else {
} omode = 1;
else imode = 0;
{ }
omode = 1; int steep = abs(y1 - y0) > abs(x1 - x0);
imode = 0; if (steep) {
} SWAP(x0, y0);
int steep = abs(y1 - y0) > abs(x1 - x0); SWAP(x1, y1);
if(steep) }
{ if (x0 > x1) {
SWAP(x0, y0); SWAP(x0, x1);
SWAP(x1, y1); SWAP(y0, y1);
} }
if(x0 > x1) int deltax = x1 - x0;
{ int deltay = abs(y1 - y0);
SWAP(x0, x1); int error = deltax / 2;
SWAP(y0, y1); int ystep;
} int y = y0;
int deltax = x1 - x0; int x;
int deltay = abs(y1 - y0); if (y0 < y1) {
int error = deltax / 2; ystep = 1;
int ystep; } else {
int y = y0; ystep = -1;
int x; }
if(y0 < y1) // Draw the outline.
ystep = 1; for (x = x0; x < x1; x++) {
else if (steep) {
ystep = -1; write_pixel_lm(y - 1, x, mmode, omode);
// Draw the outline. write_pixel_lm(y + 1, x, mmode, omode);
for(x = x0; x < x1; x++) write_pixel_lm(y, x - 1, mmode, omode);
{ write_pixel_lm(y, x + 1, mmode, omode);
if(steep) } else {
{ write_pixel_lm(x - 1, y, mmode, omode);
write_pixel_lm(y - 1, x, mmode, omode); write_pixel_lm(x + 1, y, mmode, omode);
write_pixel_lm(y + 1, x, mmode, omode); write_pixel_lm(x, y - 1, mmode, omode);
write_pixel_lm(y, x - 1, mmode, omode); write_pixel_lm(x, y + 1, mmode, omode);
write_pixel_lm(y, x + 1, mmode, omode); }
} error -= deltay;
else if (error < 0) {
{ y += ystep;
write_pixel_lm(x - 1, y, mmode, omode); error += deltax;
write_pixel_lm(x + 1, y, mmode, omode); }
write_pixel_lm(x, y - 1, mmode, omode); }
write_pixel_lm(x, y + 1, mmode, omode); // Now draw the innards.
} error = deltax / 2;
error -= deltay; y = y0;
if(error < 0) for (x = x0; x < x1; x++) {
{ if (steep) {
y += ystep; write_pixel_lm(y, x, mmode, imode);
error += deltax; } else {
} write_pixel_lm(x, y, mmode, imode);
} }
// Now draw the innards. error -= deltay;
error = deltax / 2; if (error < 0) {
y = y0; y += ystep;
for(x = x0; x < x1; x++) error += deltax;
{ }
if(steep) }
{
write_pixel_lm(y, x, mmode, imode);
}
else
{
write_pixel_lm(x, y, mmode, imode);
}
error -= deltay;
if(error < 0)
{
y += ystep;
error += deltax;
}
}
} }
/** /**
@ -991,12 +932,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
*/ */
void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode) void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode)
{ {
uint16_t firstmask = word >> xoff; uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff); uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode); WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode); WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
if(xoff > 0) if (xoff > 0) {
WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode); WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
}
} }
/** /**
@ -1016,12 +958,13 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
*/ */
void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{ {
uint16_t firstmask = word >> xoff; uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff); uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff); WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8); WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
if(xoff > 0) if (xoff > 0) {
WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8); WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
}
} }
/** /**
@ -1041,12 +984,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
*/ */
void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
{ {
uint16_t firstmask = word >> xoff; uint16_t firstmask = word >> xoff;
uint16_t lastmask = word << (16 - xoff); uint16_t lastmask = word << (16 - xoff);
WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff); WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8); WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
if(xoff > 0) if (xoff > 0) {
WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8); WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
}
} }
@ -1064,8 +1008,8 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
*/ */
void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode) void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode)
{ {
write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode); write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode); write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
} }
/** /**
@ -1076,22 +1020,22 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr,
*/ */
int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup) int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
{ {
// First locate the font struct. // First locate the font struct.
if(font > SIZEOF_ARRAY(fonts)) if (font > SIZEOF_ARRAY(fonts)) {
return 0; // font does not exist, exit.*/ return 0; // font does not exist, exit.
// Load the font info; IDs are always sequential. }
*font_info = fonts[font]; // Load the font info; IDs are always sequential.
// Locate character in font lookup table. (If required.) *font_info = fonts[font];
if(lookup != NULL) // Locate character in font lookup table. (If required.)
{ if (lookup != NULL) {
*lookup = font_info->lookup[ch]; *lookup = font_info->lookup[ch];
if(*lookup == 0xff) if (*lookup == 0xff) {
return 0; // character doesn't exist, don't bother writing it. return 0; // character doesn't exist, don't bother writing it.
} }
return 1; }
return 1;
} }
/** /**
* write_char16: Draw a character on the current draw buffer. * write_char16: Draw a character on the current draw buffer.
* Currently supports outlined characters and characters with * Currently supports outlined characters and characters with
@ -1110,7 +1054,7 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
struct FontEntry font_info; struct FontEntry font_info;
//char lookup = 0; //char lookup = 0;
fetch_font_info(0, font, &font_info, NULL); fetch_font_info(0, font, &font_info, NULL);
// Compute starting address (for x,y) of character. // Compute starting address (for x,y) of character.
int addr = CALC_BUFF_ADDR(x, y); int addr = CALC_BUFF_ADDR(x, y);
int wbit = CALC_BIT_IN_WORD(x); int wbit = CALC_BIT_IN_WORD(x);
@ -1119,58 +1063,55 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
// How big is the character? We handle characters up to 8 pixels // How big is the character? We handle characters up to 8 pixels
// wide for now. Support for large characters may be added in future. // wide for now. Support for large characters may be added in future.
{ {
// Ensure we don't overflow. // Ensure we don't overflow.
if(x + wbit > GRAPHICS_WIDTH_REAL) if (x + wbit > GRAPHICS_WIDTH_REAL) {
return; return;
// Load data pointer. }
row = ch * font_info.height; // Load data pointer.
row_temp = row; row = ch * font_info.height;
addr_temp = addr; row_temp = row;
xshift = 16 - font_info.width; addr_temp = addr;
// We can write mask words easily. xshift = 16 - font_info.width;
for(yy = y; yy < y + font_info.height; yy++) // We can write mask words easily.
{ for (yy = y; yy < y + font_info.height; yy++) {
if(font==3) if (font == 3) {
write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit); write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
else } else {
write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit); write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8; }
row++; addr += GRAPHICS_WIDTH_REAL / 8;
} row++;
// Level bits are more complicated. We need to set or clear }
// level bits, but only where the mask bit is set; otherwise, // Level bits are more complicated. We need to set or clear
// we need to leave them alone. To do this, for each word, we // level bits, but only where the mask bit is set; otherwise,
// construct an AND mask and an OR mask, and apply each individually. // we need to leave them alone. To do this, for each word, we
row = row_temp; // construct an AND mask and an OR mask, and apply each individually.
addr = addr_temp; row = row_temp;
for(yy = y; yy < y + font_info.height; yy++) addr = addr_temp;
{ for (yy = y; yy < y + font_info.height; yy++) {
if(font==3) if (font == 3) {
{ level_bits = font_frame12x18[row];
level_bits = font_frame12x18[row]; //if(!(flags & FONT_INVERT)) // data is normally inverted
//if(!(flags & FONT_INVERT)) // data is normally inverted level_bits = ~level_bits;
level_bits = ~level_bits; or_mask = font_mask12x18[row] << xshift;
or_mask = font_mask12x18[row] << xshift; and_mask = (font_mask12x18[row] & level_bits) << xshift;
and_mask = (font_mask12x18[row] & level_bits) << xshift; } else {
} else { level_bits = font_frame8x10[row];
level_bits = font_frame8x10[row]; //if(!(flags & FONT_INVERT)) // data is normally inverted
//if(!(flags & FONT_INVERT)) // data is normally inverted level_bits = ~level_bits;
level_bits = ~level_bits; or_mask = font_mask8x10[row] << xshift;
or_mask = font_mask8x10[row] << xshift; and_mask = (font_mask8x10[row] & level_bits) << xshift;
and_mask = (font_mask8x10[row] & level_bits) << xshift; }
} write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); // If we're not bold write the AND mask.
// If we're not bold write the AND mask. //if(!(flags & FONT_BOLD))
//if(!(flags & FONT_BOLD)) write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8;
addr += GRAPHICS_WIDTH_REAL / 8; row++;
row++; }
}
} }
} }
/** /**
* write_char: Draw a character on the current draw buffer. * write_char: Draw a character on the current draw buffer.
* Currently supports outlined characters and characters with * Currently supports outlined characters and characters with
@ -1195,98 +1136,98 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
// If font only supports lowercase or uppercase, make the letter // If font only supports lowercase or uppercase, make the letter
// lowercase or uppercase. // lowercase or uppercase.
/*if(font_info.flags & FONT_LOWERCASE_ONLY) /*if(font_info.flags & FONT_LOWERCASE_ONLY)
ch = tolower(ch); ch = tolower(ch);
if(font_info.flags & FONT_UPPERCASE_ONLY) if(font_info.flags & FONT_UPPERCASE_ONLY)
ch = toupper(ch);*/ ch = toupper(ch);*/
fetch_font_info(ch, font, &font_info, &lookup); fetch_font_info(ch, font, &font_info, &lookup);
// How big is the character? We handle characters up to 8 pixels // How big is the character? We handle characters up to 8 pixels
// wide for now. Support for large characters may be added in future. // wide for now. Support for large characters may be added in future.
if(font_info.width <= 8) if (font_info.width <= 8) {
{ // Ensure we don't overflow.
// Ensure we don't overflow. if (x + wbit > GRAPHICS_WIDTH_REAL) {
if(x + wbit > GRAPHICS_WIDTH_REAL) return;
return; }
// Load data pointer. // Load data pointer.
row = lookup * font_info.height * 2; row = lookup * font_info.height * 2;
row_temp = row; row_temp = row;
addr_temp = addr; addr_temp = addr;
xshift = 16 - font_info.width; xshift = 16 - font_info.width;
// We can write mask words easily. // We can write mask words easily.
for(yy = y; yy < y + font_info.height; yy++) for (yy = y; yy < y + font_info.height; yy++) {
{ write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit); addr += GRAPHICS_WIDTH_REAL / 8;
addr += GRAPHICS_WIDTH_REAL / 8; row++;
row++; }
} // Level bits are more complicated. We need to set or clear
// Level bits are more complicated. We need to set or clear // level bits, but only where the mask bit is set; otherwise,
// level bits, but only where the mask bit is set; otherwise, // we need to leave them alone. To do this, for each word, we
// we need to leave them alone. To do this, for each word, we // construct an AND mask and an OR mask, and apply each individually.
// construct an AND mask and an OR mask, and apply each individually. row = row_temp;
row = row_temp; addr = addr_temp;
addr = addr_temp; for (yy = y; yy < y + font_info.height; yy++) {
for(yy = y; yy < y + font_info.height; yy++) level_bits = font_info.data[row + font_info.height];
{ if (!(flags & FONT_INVERT)) {
level_bits = font_info.data[row + font_info.height]; // data is normally inverted
if(!(flags & FONT_INVERT)) // data is normally inverted level_bits = ~level_bits;
level_bits = ~level_bits; }
or_mask = font_info.data[row] << xshift; or_mask = font_info.data[row] << xshift;
and_mask = (font_info.data[row] & level_bits) << xshift; and_mask = (font_info.data[row] & level_bits) << xshift;
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
// If we're not bold write the AND mask. // If we're not bold write the AND mask.
//if(!(flags & FONT_BOLD)) //if(!(flags & FONT_BOLD))
write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
addr += GRAPHICS_WIDTH_REAL / 8; addr += GRAPHICS_WIDTH_REAL / 8;
row++; row++;
} }
} }
} }
/** /**
* calc_text_dimensions: Calculate the dimensions of a * calc_text_dimensions: Calculate the dimensions of a
* string in a given font. Supports new lines and * string in a given font. Supports new lines and
* carriage returns in text. * carriage returns in text.
* *
* @param str string to calculate dimensions of * @param str string to calculate dimensions of
* @param font_info font info structure * @param font_info font info structure
* @param xs horizontal spacing * @param xs horizontal spacing
* @param ys vertical spacing * @param ys vertical spacing
* @param dim return result: struct FontDimensions * @param dim return result: struct FontDimensions
*/ */
void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim) void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim)
{ {
int max_length = 0, line_length = 0, lines = 1; int max_length = 0, line_length = 0, lines = 1;
while(*str != 0) while (*str != 0) {
{ line_length++;
line_length++; if (*str == '\n' || *str == '\r') {
if(*str == '\n' || *str == '\r') if (line_length > max_length) {
{ max_length = line_length;
if(line_length > max_length) }
max_length = line_length; line_length = 0;
line_length = 0; lines++;
lines++; }
} str++;
str++; }
if (line_length > max_length) {
max_length = line_length;
} }
if(line_length > max_length)
max_length = line_length;
dim->width = max_length * (font.width + xs); dim->width = max_length * (font.width + xs);
dim->height = lines * (font.height + ys); dim->height = lines * (font.height + ys);
} }
/** /**
* write_string: Draw a string on the screen with certain * write_string: Draw a string on the screen with certain
* alignment parameters. * alignment parameters.
* *
* @param str string to write * @param str string to write
* @param x x coordinate * @param x x coordinate
* @param y y coordinate * @param y y coordinate
* @param xs horizontal spacing * @param xs horizontal spacing
* @param ys horizontal spacing * @param ys horizontal spacing
* @param va vertical align * @param va vertical align
* @param ha horizontal align * @param ha horizontal align
* @param flags flags (passed to write_char) * @param flags flags (passed to write_char)
* @param font font * @param font font
*/ */
void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font) void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font)
{ {
int xx = 0, yy = 0, xx_original = 0; int xx = 0, yy = 0, xx_original = 0;
@ -1295,55 +1236,61 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
// Determine font info and dimensions/position of the string. // Determine font info and dimensions/position of the string.
fetch_font_info(0, font, &font_info, NULL); fetch_font_info(0, font, &font_info, NULL);
calc_text_dimensions(str, font_info, xs, ys, &dim); calc_text_dimensions(str, font_info, xs, ys, &dim);
switch(va) switch (va) {
{ case TEXT_VA_TOP:
case TEXT_VA_TOP: yy = y; break; yy = y;
case TEXT_VA_MIDDLE: yy = y - (dim.height / 2); break; break;
case TEXT_VA_BOTTOM: yy = y - dim.height; break; case TEXT_VA_MIDDLE:
yy = y - (dim.height / 2);
break;
case TEXT_VA_BOTTOM:
yy = y - dim.height;
break;
} }
switch(ha) switch (ha) {
{ case TEXT_HA_LEFT:
case TEXT_HA_LEFT: xx = x; break; xx = x;
case TEXT_HA_CENTER: xx = x - (dim.width / 2); break; break;
case TEXT_HA_RIGHT: xx = x - dim.width; break; case TEXT_HA_CENTER:
xx = x - (dim.width / 2);
break;
case TEXT_HA_RIGHT:
xx = x - dim.width;
break;
} }
// Then write each character. // Then write each character.
xx_original = xx; xx_original = xx;
while(*str != 0) while (*str != 0) {
{ if (*str == '\n' || *str == '\r') {
if(*str == '\n' || *str == '\r') yy += ys + font_info.height;
{ xx = xx_original;
yy += ys + font_info.height; } else {
xx = xx_original; if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
} if (font_info.id < 2) {
else write_char(*str, xx, yy, flags, font);
{ } else {
if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL) write_char16(*str, xx, yy, font);
{ }
if(font_info.id<2) }
write_char(*str, xx, yy, flags, font); xx += font_info.width + xs;
else }
write_char16(*str, xx, yy, font); str++;
}
xx += font_info.width + xs;
}
str++;
} }
} }
/** /**
* write_string_formatted: Draw a string with format escape * write_string_formatted: Draw a string with format escape
* sequences in it. Allows for complex text effects. * sequences in it. Allows for complex text effects.
* *
* @param str string to write (with format data) * @param str string to write (with format data)
* @param x x coordinate * @param x x coordinate
* @param y y coordinate * @param y y coordinate
* @param xs default horizontal spacing * @param xs default horizontal spacing
* @param ys default horizontal spacing * @param ys default horizontal spacing
* @param va vertical align * @param va vertical align
* @param ha horizontal align * @param ha horizontal align
* @param flags flags (passed to write_char) * @param flags flags (passed to write_char)
*/ */
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags) void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags)
{ {
int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0; int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
@ -1360,78 +1307,76 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
// work out a bounding box. We'll parse again for the final output. // work out a bounding box. We'll parse again for the final output.
// This is a simple state machine parser. // This is a simple state machine parser.
char *ostr = str; char *ostr = str;
while(*str) while (*str) {
{ if (*str == '<' && fcode == 1) {
if(*str == '<' && fcode == 1) // escape code: skip // escape code: skip
fcode = 0; fcode = 0;
if(*str == '<' && fcode == 0) // begin format code? }
{ if (*str == '<' && fcode == 0) {
fcode = 1; // begin format code?
fptr = 0; fcode = 1;
fptr = 0;
}
if (*str == '>' && fcode == 1) {
fcode = 0;
if (strcmp(fstack, "B")) {
// switch to "big" font (font #1)
fwidth = bigfontwidth;
fheight = bigfontheight;
} else if (strcmp(fstack, "S")) {
// switch to "small" font (font #0)
fwidth = smallfontwidth;
fheight = smallfontheight;
} }
if(*str == '>' && fcode == 1) if (fheight > max_height) {
{ max_height = fheight;
fcode = 0;
if(strcmp(fstack, "B")) // switch to "big" font (font #1)
{
fwidth = bigfontwidth;
fheight = bigfontheight;
}
else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
{
fwidth = smallfontwidth;
fheight = smallfontheight;
}
if(fheight > max_height)
max_height = fheight;
// Skip over this byte. Go to next byte.
str++;
continue;
}
if(*str != '<' && *str != '>' && fcode == 1)
{
// Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes
{
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if(fcode == 0)
{
// Not a format code, raw text.
xx += fwidth + xs;
if(*str == '\n')
{
if(xx > max_xx)
max_xx = xx;
xx = x;
yy += fheight + ys;
}
} }
// Skip over this byte. Go to next byte.
str++; str++;
continue;
}
if (*str != '<' && *str != '>' && fcode == 1) {
// Add to the format stack (up to 10 bytes.)
if (fptr > 10) {
// stop adding bytes
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if (fcode == 0) {
// Not a format code, raw text.
xx += fwidth + xs;
if (*str == '\n') {
if (xx > max_xx) {
max_xx = xx;
}
xx = x;
yy += fheight + ys;
}
}
str++;
} }
// Reset string pointer. // Reset string pointer.
str = ostr; str = ostr;
// Now we've parsed it and got a bbox, we need to work out the dimensions of it // Now we've parsed it and got a bbox, we need to work out the dimensions of it
// and how to align it. // and how to align it.
/*int width = max_xx - x; /*int width = max_xx - x;
int height = yy - y; int height = yy - y;
int ay, ax; int ay, ax;
switch(va) switch(va)
{ {
case TEXT_VA_TOP: ay = yy; break; case TEXT_VA_TOP: ay = yy; break;
case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; case TEXT_VA_MIDDLE: ay = yy - (height / 2); break;
case TEXT_VA_BOTTOM: ay = yy - height; break; case TEXT_VA_BOTTOM: ay = yy - height; break;
} }
switch(ha) switch(ha)
{ {
case TEXT_HA_LEFT: ax = x; break; case TEXT_HA_LEFT: ax = x; break;
case TEXT_HA_CENTER: ax = x - (width / 2); break; case TEXT_HA_CENTER: ax = x - (width / 2); break;
case TEXT_HA_RIGHT: ax = x - width; break; case TEXT_HA_RIGHT: ax = x - width; break;
}*/ }*/
// So ax,ay is our new text origin. Parse the text format again and paint // So ax,ay is our new text origin. Parse the text format again and paint
// the text on the display. // the text on the display.
fcode = 0; fcode = 0;
@ -1439,204 +1384,196 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
font = 0; font = 0;
xx = 0; xx = 0;
yy = 0; yy = 0;
while(*str) while (*str) {
{ if (*str == '<' && fcode == 1) {
if(*str == '<' && fcode == 1) // escape code: skip // escape code: skip
fcode = 0; fcode = 0;
if(*str == '<' && fcode == 0) // begin format code? }
{ if (*str == '<' && fcode == 0) {
fcode = 1; // begin format code?
fptr = 0; fcode = 1;
} fptr = 0;
if(*str == '>' && fcode == 1) }
{ if (*str == '>' && fcode == 1) {
fcode = 0; fcode = 0;
if(strcmp(fstack, "B")) // switch to "big" font (font #1) if (strcmp(fstack, "B")) {
{ // switch to "big" font (font #1)
fwidth = bigfontwidth; fwidth = bigfontwidth;
fheight = bigfontheight; fheight = bigfontheight;
font = 1; font = 1;
} } else if (strcmp(fstack, "S")) {
else if(strcmp(fstack, "S")) // switch to "small" font (font #0) // switch to "small" font (font #0)
{ fwidth = smallfontwidth;
fwidth = smallfontwidth; fheight = smallfontheight;
fheight = smallfontheight; font = 0;
font = 0;
}
// Skip over this byte. Go to next byte.
str++;
continue;
}
if(*str != '<' && *str != '>' && fcode == 1)
{
// Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes
{
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if(fcode == 0)
{
// Not a format code, raw text. So we draw it.
// TODO - different font sizes.
write_char(*str, xx, yy + (max_height - fheight), flags, font);
xx += fwidth + xs;
if(*str == '\n')
{
if(xx > max_xx)
max_xx = xx;
xx = x;
yy += fheight + ys;
}
} }
// Skip over this byte. Go to next byte.
str++; str++;
continue;
}
if (*str != '<' && *str != '>' && fcode == 1) {
// Add to the format stack (up to 10 bytes.)
if (fptr > 10) {
// stop adding bytes
str++; // go to next byte
continue;
}
fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
}
if (fcode == 0) {
// Not a format code, raw text. So we draw it.
// TODO - different font sizes.
write_char(*str, xx, yy + (max_height - fheight), flags, font);
xx += fwidth + xs;
if (*str == '\n') {
if (xx > max_xx) {
max_xx = xx;
}
xx = x;
yy += fheight + ys;
}
}
str++;
} }
} }
//SUPEROSD- //SUPEROSD-
// graphics // graphics
void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size) void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size)
{ {
int16_t a = mySin(roll+360); int16_t a = mySin(roll + 360);
int16_t b = myCos(roll+360); int16_t b = myCos(roll + 360);
int16_t c = mySin(roll+90+360)*5/100; int16_t c = mySin(roll + 90 + 360) * 5 / 100;
int16_t d = myCos(roll+90+360)*5/100; int16_t d = myCos(roll + 90 + 360) * 5 / 100;
int16_t k; int16_t k;
int16_t l; int16_t l;
int16_t indi30x1=myCos(30)*(size/2+1) / 100; int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100;
int16_t indi30y1=mySin(30)*(size/2+1) / 100; int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100;
int16_t indi30x2=myCos(30)*(size/2+4) / 100; int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100;
int16_t indi30y2=mySin(30)*(size/2+4) / 100; int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100;
int16_t indi60x1=myCos(60)*(size/2+1) / 100; int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100;
int16_t indi60y1=mySin(60)*(size/2+1) / 100; int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100;
int16_t indi60x2=myCos(60)*(size/2+4) / 100; int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100;
int16_t indi60y2=mySin(60)*(size/2+4) / 100; int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100;
pitch=pitch%90; pitch = pitch % 90;
if(pitch>90) if (pitch > 90) {
{ pitch = pitch - 90;
pitch=pitch-90; }
} if (pitch < -90) {
if(pitch<-90) pitch = pitch + 90;
{ }
pitch=pitch+90; a = (a * (size / 2)) / 100;
} b = (b * (size / 2)) / 100;
a = (a * (size/2)) / 100;
b = (b * (size/2)) / 100;
if(roll<-90 || roll>90) if (roll < -90 || roll > 90) {
pitch=pitch*-1; pitch = pitch * -1;
k = a*pitch/90; }
l = b*pitch/90; k = a * pitch / 90;
l = b * pitch / 90;
// scale // scale
//0 //0
//drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
//drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
write_line_outlined((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1,0,0,0,1); write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
write_line_outlined((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1,0,0,0,1); write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
//30 //30
//drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
//drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
write_line_outlined((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2,0,0,0,1); write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
write_line_outlined((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2,0,0,0,1); write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
//60 //60
//drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
//drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
write_line_outlined((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2,0,0,0,1); write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
write_line_outlined((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2,0,0,0,1); write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
//90 //90
//drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
write_line_outlined((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1),0,0,0,1); write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1);
//roll
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line
//"wingtips"
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1);
write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);
//roll //pitch
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a,0,0,0,1); //Direction line write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1);
//"wingtips"
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c,0,0,0,1);
write_line_outlined((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a,0,0,0,1);
//pitch //drawCircle(x-1, y-1, 5);
//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); //write_circle_outlined(x-1, y-1, 5,0,0,0,1);
write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,0,0,0,1); //drawCircle(x-1, y-1, size/2+4);
//write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
//drawCircle(x-1, y-1, 5);
//write_circle_outlined(x-1, y-1, 5,0,0,0,1);
//drawCircle(x-1, y-1, size/2+4);
//write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
} }
void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size) void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
{ {
int i=0; int i = 0;
int batteryLines; int batteryLines;
//top //top
/*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1); /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);
drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
//bottom //bottom
drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
//left //left
drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);
//right //right
drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
write_rectangle_outlined((x)-1, (y)-1+2,size,size*3,0,1); write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1);
write_vline_lm((x)-1+(size/2+size/4)+1,(y)-2,(y)-1+1,0,1); write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1);
write_vline_lm((x)-1+(size/2-size/4)-1,(y)-2,(y)-1+1,0,1); write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-2,0,1); write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1,1,1); write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1);
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1+1,1,1); write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1);
batteryLines = battery*(size*3-2)/100; batteryLines = battery * (size * 3 - 2) / 100;
for(i=0;i<batteryLines;i++) for (i = 0; i < batteryLines; i++) {
{ write_hline_lm((x) - 1, (x) - 1 + size, (y) - 1 + size * 3 - i, 1, 1);
write_hline_lm((x)-1,(x)-1 + size,(y)-1+size*3-i,1,1); }
}
} }
void printTime(uint16_t x, uint16_t y)
void printTime(uint16_t x, uint16_t y) { {
char temp[9]={0}; char temp[9] =
sprintf(temp,"%02d:%02d:%02d",timex.hour,timex.min,timex.sec); { 0 };
//printTextFB(x,y,temp); sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); //printTextFB(x,y,temp);
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
} }
/* /*
void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) { void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
char temp[9]={0}; char temp[9]={0};
char updown=' '; char updown=' ';
uint16_t charx=x/16; uint16_t charx=x/16;
if(dir==0) if(dir==0)
updown=24; updown=24;
if(dir==1) if(dir==1)
updown=25; updown=25;
sprintf(temp,"%c%6dm",updown,alt); sprintf(temp,"%c%6dm",updown,alt);
printTextFB(charx,y+2,temp); printTextFB(charx,y+2,temp);
// frame // frame
drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11); drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
}*/ }*/
/** /**
* hud_draw_vertical_scale: Draw a vertical scale. * hud_draw_vertical_scale: Draw a vertical scale.
@ -1655,148 +1592,134 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
* @param max_val maximum expected value (used to compute size of arrow ticker) * @param max_val maximum expected value (used to compute size of arrow ticker)
* @param flags special flags (see hud.h.) * @param flags special flags (see hud.h.)
*/ */
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int boundtick_len, int max_val, int flags) void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
int boundtick_len, int max_val, int flags)
{ {
char temp[15];//, temp2[15]; char temp[15]; //, temp2[15];
struct FontEntry font_info; struct FontEntry font_info;
struct FontDimensions dim; struct FontDimensions dim;
// Halign should be in a small span. // Halign should be in a small span.
//MY_ASSERT(halign >= -1 && halign <= 1); //MY_ASSERT(halign >= -1 && halign <= 1);
// Compute the position of the elements. // Compute the position of the elements.
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0; int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
if(halign == -1) if (halign == -1) {
{ majtick_start = x;
majtick_start = x; majtick_end = x + majtick_len;
majtick_end = x + majtick_len; mintick_start = x;
mintick_start = x; mintick_end = x + mintick_len;
mintick_end = x + mintick_len; boundtick_start = x;
boundtick_start = x; boundtick_end = x + boundtick_len;
boundtick_end = x + boundtick_len; } else if (halign == +1) {
} x = x - GRAPHICS_HDEADBAND;
else if(halign == +1) majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
{ majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
x=x-GRAPHICS_HDEADBAND; mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
majtick_start = GRAPHICS_WIDTH_REAL - x - 1; mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
mintick_start = GRAPHICS_WIDTH_REAL - x - 1; boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; }
boundtick_start = GRAPHICS_WIDTH_REAL - x - 1; // Retrieve width of large font (font #0); from this calculate the x spacing.
boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; fetch_font_info(0, 0, &font_info, NULL);
} int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly??
// Retrieve width of large font (font #0); from this calculate the x spacing. int text_x_spacing = arrow_len;
fetch_font_info(0, 0, &font_info, NULL); int max_text_y = 0, text_length = 0;
int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
int text_x_spacing = arrow_len; // For -(range / 2) to +(range / 2), draw the scale.
int max_text_y = 0, text_length = 0; int range_2 = range / 2; //, height_2 = height / 2;
int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1 int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
// For -(range / 2) to +(range / 2), draw the scale. // Iterate through each step.
int range_2 = range / 2; //, height_2 = height / 2; for (r = -range_2; r <= +range_2; r++) {
int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, style = 0;
// Iterate through each step. rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
for(r = -range_2; r <= +range_2; r++) rv = -rr + range_2; // for number display
{ if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
style = 0; rr += majtick_step / 2;
rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape if (rr % majtick_step == 0)
rv = -rr + range_2; // for number display style = 1; // major tick
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE) else if (rr % mintick_step == 0)
rr += majtick_step / 2; style = 2; // minor tick
if(rr % majtick_step == 0) else
style = 1; // major tick style = 0;
else if(rr % mintick_step == 0) if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
style = 2; // minor tick continue;
if (style) {
// Calculate y position.
ys = ((long int) (r * height) / (long int) range) + y;
//sprintf(temp, "ys=%d", ys);
//con_puts(temp, 0);
// Depending on style, draw a minor or a major tick.
if (style == 1) {
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
memset(temp, ' ', 10);
//my_itoa(rv, temp);
sprintf(temp, "%d", rv);
text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
if (text_length > max_text_y)
max_text_y = text_length;
if (halign == -1)
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
else else
style = 0; write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) } else if (style == 2)
continue; write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
if(style)
{
// Calculate y position.
ys = ((long int)(r * height) / (long int)range) + y;
//sprintf(temp, "ys=%d", ys);
//con_puts(temp, 0);
// Depending on style, draw a minor or a major tick.
if(style == 1)
{
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
memset(temp, ' ', 10);
//my_itoa(rv, temp);
sprintf(temp,"%d",rv);
text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
if(text_length > max_text_y)
max_text_y = text_length;
if(halign == -1)
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
else
write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
}
else if(style == 2)
write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
}
} }
// Generate the string for the value, as well as calculating its dimensions. }
memset(temp, ' ', 10); // Generate the string for the value, as well as calculating its dimensions.
//my_itoa(v, temp); memset(temp, ' ', 10);
sprintf(temp,"%d",v); //my_itoa(v, temp);
// TODO: add auto-sizing. sprintf(temp, "%d", v);
calc_text_dimensions(temp, font_info, 1, 0, &dim); // TODO: add auto-sizing.
int xx = 0, i = 0; calc_text_dimensions(temp, font_info, 1, 0, &dim);
if(halign == -1) int xx = 0, i = 0;
xx = majtick_end + text_x_spacing; if (halign == -1)
else xx = majtick_end + text_x_spacing;
xx = majtick_end - text_x_spacing; else
// Draw an arrow from the number to the point. xx = majtick_end - text_x_spacing;
for(i = 0; i < arrow_len; i++) // Draw an arrow from the number to the point.
{ for (i = 0; i < arrow_len; i++) {
if(halign == -1) if (halign == -1) {
{ write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1); write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1); write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1); write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1); } else {
} write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
else write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
{ write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1); write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
}
// FIXME
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
} }
if(halign == -1) // FIXME
{ // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1); // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1); }
write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); if (halign == -1) {
} write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
else write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
{ write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1); } else {
write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1); write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
} write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
// Draw the text. }
if(halign == -1) // Draw the text.
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0); if (halign == -1)
else write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0); else
// Then, add a slow cut off on the edges, so the text doesn't sharply write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
// disappear. We simply clear the areas above and below the ticker, and we // Then, add a slow cut off on the edges, so the text doesn't sharply
// use little markers on the edges. // disappear. We simply clear the areas above and below the ticker, and we
if(halign == -1) // use little markers on the edges.
{ if (halign == -1) {
write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0); write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0); 0);
} write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
else 0);
{ } else {
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
} }
write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1); write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1); write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
} }
/** /**
@ -1815,613 +1738,577 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
*/ */
void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags) void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags)
{ {
v %= 360; // wrap, just in case. v %= 360; // wrap, just in case.
struct FontEntry font_info; struct FontEntry font_info;
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0; int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
char headingstr[4]; char headingstr[4];
majtick_start = y; majtick_start = y;
majtick_end = y - majtick_len; majtick_end = y - majtick_len;
mintick_start = y; mintick_start = y;
mintick_end = y - mintick_len; mintick_end = y - mintick_len;
textoffset = 8; textoffset = 8;
int r, style, rr, xs; // rv, int r, style, rr, xs; // rv,
int range_2 = range / 2; int range_2 = range / 2;
for(r = -range_2; r <= +range_2; r++) for (r = -range_2; r <= +range_2; r++) {
{ style = 0;
style = 0; rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track //rv = -rr + range_2; // for number display
//rv = -rr + range_2; // for number display if (rr % majtick_step == 0)
if(rr % majtick_step == 0) style = 1; // major tick
style = 1; // major tick else if (rr % mintick_step == 0)
else if(rr % mintick_step == 0) style = 2; // minor tick
style = 2; // minor tick if (style) {
if(style) // Calculate x position.
{ xs = ((long int) (r * width) / (long int) range) + x;
// Calculate x position. // Draw it.
xs = ((long int)(r * width) / (long int)range) + x; if (style == 1) {
// Draw it. write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
if(style == 1) // Draw heading above this tick.
{ // If it's not one of north, south, east, west, draw the heading.
write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); // Otherwise, draw one of the identifiers.
// Draw heading above this tick. if (rr % 90 != 0) {
// If it's not one of north, south, east, west, draw the heading. // We abbreviate heading to two digits. This has the side effect of being easy to compute.
// Otherwise, draw one of the identifiers. headingstr[0] = '0' + (rr / 100);
if(rr % 90 != 0) headingstr[1] = '0' + ((rr / 10) % 10);
{ headingstr[2] = 0;
// We abbreviate heading to two digits. This has the side effect of being easy to compute. headingstr[3] = 0; // nul to terminate
headingstr[0] = '0' + (rr / 100); } else {
headingstr[1] = '0' + ((rr / 10) % 10); switch (rr) {
headingstr[2] = 0; case 0:
headingstr[3] = 0; // nul to terminate headingstr[0] = 'N';
} break;
else case 90:
{ headingstr[0] = 'E';
switch(rr) break;
{ case 180:
case 0: headingstr[0] = 'N'; break; headingstr[0] = 'S';
case 90: headingstr[0] = 'E'; break; break;
case 180: headingstr[0] = 'S'; break; case 270:
case 270: headingstr[0] = 'W'; break; headingstr[0] = 'W';
} break;
headingstr[1] = 0; }
headingstr[2] = 0; headingstr[1] = 0;
headingstr[3] = 0; headingstr[2] = 0;
} headingstr[3] = 0;
// +1 fudge...!
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
}
else if(style == 2)
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
} }
// +1 fudge...!
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
} else if (style == 2)
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
} }
// Then, draw a rectangle with the present heading in it. }
// We want to cover up any other markers on the bottom. // Then, draw a rectangle with the present heading in it.
// First compute font size. // We want to cover up any other markers on the bottom.
fetch_font_info(0, 3, &font_info, NULL); // First compute font size.
int text_width = (font_info.width + 1) * 3; fetch_font_info(0, 3, &font_info, NULL);
int rect_width = text_width + 2; int text_width = (font_info.width + 1) * 3;
write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); int rect_width = text_width + 2;
write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
headingstr[0] = '0' + (v / 100); write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
headingstr[1] = '0' + ((v / 10) % 10); headingstr[0] = '0' + (v / 100);
headingstr[2] = '0' + (v % 10); headingstr[1] = '0' + ((v / 10) % 10);
headingstr[3] = 0; headingstr[2] = '0' + (v % 10);
write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3); headingstr[3] = 0;
write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
} }
// CORE draw routines end here // CORE draw routines end here
void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size ) void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size)
{ {
float alpha; float alpha;
uint8_t vertical=0,horizontal=0; uint8_t vertical = 0, horizontal = 0;
int16_t x1,x2; int16_t x1, x2;
int16_t y1,y2; int16_t y1, y2;
int16_t refx,refy; int16_t refx, refy;
alpha=DEG2RAD(angle); alpha = DEG2RAD(angle);
refx=l_x + size/2; refx = l_x + size / 2;
refy=l_y + size/2; refy = l_y + size / 2;
// //
float k=0; float k = 0;
float dx = sinf(alpha)*(pitch/90.0f*(size/2)); float dx = sinf(alpha) * (pitch / 90.0f * (size / 2));
float dy = cosf(alpha)*(pitch/90.0f*(size/2)); float dy = cosf(alpha) * (pitch / 90.0f * (size / 2));
int16_t x0 = (size/2)-dx; int16_t x0 = (size / 2) - dx;
int16_t y0 = (size/2)+dy; int16_t y0 = (size / 2) + dy;
// calculate the line function // calculate the line function
if((angle != 90) && (angle != -90)) if ((angle != 90) && (angle != -90)) {
{ k = tanf(alpha);
k = tanf(alpha); vertical = 0;
vertical = 0; if (k == 0) {
if(k==0) horizontal = 1;
{ }
horizontal=1; } else {
} vertical = 1;
} }
else
{
vertical = 1;
}
// crossing point of line // crossing point of line
if(!vertical && !horizontal) if (!vertical && !horizontal) {
{ // y-y0=k(x-x0)
// y-y0=k(x-x0) int16_t x = 0;
int16_t x=0; int16_t y = k * (x - x0) + y0;
int16_t y=k*(x-x0)+y0; // find right crossing point
// find right crossing point x1 = x;
x1=x; y1 = y;
y1=y; if (y < 0) {
if(y<0) y1 = 0;
{ x1 = ((y1 - y0) + k * x0) / k;
y1=0; }
x1=((y1-y0)+k*x0)/k; if (y > size) {
} y1 = size;
if(y>size) x1 = ((y1 - y0) + k * x0) / k;
{ }
y1=size; // left crossing point
x1=((y1-y0)+k*x0)/k; x = size;
} y = k * (x - x0) + y0;
// left crossing point x2 = x;
x=size; y2 = y;
y=k*(x-x0)+y0; if (y < 0) {
x2=x; y2 = 0;
y2=y; x2 = ((y2 - y0) + k * x0) / k;
if(y<0) }
{ if (y > size) {
y2=0; y2 = size;
x2=((y2-y0)+k*x0)/k; x2 = ((y2 - y0) + k * x0) / k;
} }
if(y>size) // move to location
{ // horizon line
y2=size; write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
x2=((y2-y0)+k*x0)/k; //fill
} if (angle <= 0 && angle > -90) {
// move to location //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
// horizon line for (int i = y2; i < size; i++) {
write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1); x2 = ((i - y0) + k * x0) / k;
//fill if (x2 > size) {
if(angle<=0 && angle>-90) x2 = size;
{ }
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); if (x2 < 0) {
for(int i=y2;i<size;i++) x2 = 0;
{ }
x2=((i-y0)+k*x0)/k; write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
if(x2>size) }
x2=size; } else if (angle < -90) {
if(x2<0) //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
x2=0; for (int i = 0; i < y2; i++) {
write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1); x2 = ((i - y0) + k * x0) / k;
} if (x2 > size) {
} x2 = size;
else if(angle<-90) }
{ if (x2 < 0) {
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); x2 = 0;
for(int i=0;i<y2;i++) }
{ write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
x2=((i-y0)+k*x0)/k; }
if(x2>size) } else if (angle > 0 && angle < 90) {
x2=size; //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
if(x2<0) for (int i = y1; i < size; i++) {
x2=0; x2 = ((i - y0) + k * x0) / k;
write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1); if (x2 > size) {
} x2 = size;
} }
else if(angle>0 && angle<90) if (x2 < 0) {
{ x2 = 0;
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); }
for(int i=y1;i<size;i++) write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
{ }
x2=((i-y0)+k*x0)/k; } else if (angle > 90) {
if(x2>size) //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
x2=size; for (int i = 0; i < y1; i++) {
if(x2<0) x2 = ((i - y0) + k * x0) / k;
x2=0; if (x2 > size) {
write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1); x2 = size;
} }
} if (x2 < 0) {
else if(angle>90) x2 = 0;
{ }
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
for(int i=0;i<y1;i++) }
{ }
x2=((i-y0)+k*x0)/k; } else if (vertical) {
if(x2>size) // horizon line
x2=size; write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
if(x2<0) if (angle == 90) {
x2=0; //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1); for (int i = 0; i < size; i++) {
} write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
} }
} } else {
else if(vertical) //write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
{ for (int i = 0; i < size; i++) {
// horizon line write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1); }
if(angle==90) }
{ } else if (horizontal) {
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); // horizon line
for(int i=0;i<size;i++) write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
{ if (angle < 0) {
write_hline_lm(0+l_x,x0+l_x,i+l_y,1,1); //write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
} for (int i = 0; i < y0; i++) {
} write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
else }
{ } else {
//write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<size;i++) for (int i = y0; i < size; i++) {
{ write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
write_hline_lm(size+l_x,x0+l_x,i+l_y,1,1); }
} }
} }
}
else if(horizontal)
{
// horizon line
write_hline_outlined(0+l_x,size+l_x,y0+l_y,0,0,0,1);
if(angle<0)
{
//write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=0;i<y0;i++)
{
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
}
}
else
{
//write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y0;i<size;i++)
{
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
}
}
}
//sides //sides
write_line_outlined(l_x,l_y,l_x,l_y+size,0,0,0,1); write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1);
write_line_outlined(l_x+size,l_y,l_x+size,l_y+size,0,0,0,1); write_line_outlined(l_x + size, l_y, l_x + size, l_y +s ize, 0, 0, 0, 1);
//plane //plane
write_line_outlined(refx-5,refy,refx+6,refy,0,0,0,1); write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1);
write_line_outlined(refx,refy,refx,refy-3,0,0,0,1); write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1);
} }
void introText()
void introText(){ {
write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
} }
void introGraphics() { void introGraphics()
/* logo */ {
int image=0; /* logo */
struct splashEntry splash_info; int image = 0;
splash_info = splash[image]; struct splashEntry splash_info;
splash_info = splash[image];
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image); copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);
/* frame */ /* frame */
drawBox(APPLY_HDEADBAND(0),APPLY_VDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT-8),APPLY_VDEADBAND(GRAPHICS_BOTTOM)); drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM));
// Must mask out last half-word because SPI keeps clocking it out otherwise // Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) { for (uint32_t i = 0; i < 8; i++) {
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
} }
} }
void calcHomeArrow(int16_t m_yaw) void calcHomeArrow(int16_t m_yaw)
{ {
HomeLocationData home; HomeLocationData home;
HomeLocationGet (&home); HomeLocationGet(&home);
GPSPositionData gpsData; GPSPositionData gpsData;
GPSPositionGet (&gpsData); GPSPositionGet(&gpsData);
/** http://www.movable-type.co.uk/scripts/latlong.html **/ /** http://www.movable-type.co.uk/scripts/latlong.html **/
float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g; float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
float elevation; float elevation;
float gcsAlt=home.Altitude; // Home MSL altitude float gcsAlt = home.Altitude; // Home MSL altitude
float uavAlt=gpsData.Altitude; // UAV MSL altitude float uavAlt = gpsData.Altitude; // UAV MSL altitude
float dAlt=uavAlt-gcsAlt; // Altitude difference float dAlt = uavAlt - gcsAlt; // Altitude difference
// Convert to radians // Convert to radians
lat1 = DEG2RAD(home.Latitude)/10000000.0f; // Home lat lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat
lon1 = DEG2RAD(home.Longitude)/10000000.0f; // Home lon lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon
lat2 = DEG2RAD(gpsData.Latitude)/10000000.0f; // UAV lat lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat
lon2 = DEG2RAD(gpsData.Longitude)/10000000.0f; // UAV lon lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon
// Bearing // Bearing
/** /**
var y = Math.sin(dLon) * Math.cos(lat2); var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) - var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg(); var brng = Math.atan2(y, x).toDeg();
**/ **/
y = sinf(lon2-lon1) * cosf(lat2); y = sinf(lon2 - lon1) * cosf(lat2);
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2-lon1); x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
brng = RAD2DEG(atan2f(y,x)); brng = RAD2DEG(atan2f(y,x));
if(brng<0) if (brng < 0) {
brng+=360; brng += 360;
}
// yaw corrected bearing, needs compass // yaw corrected bearing, needs compass
u2g=brng-180-m_yaw; u2g = brng - 180 - m_yaw;
if(u2g<0) if (u2g < 0) {
u2g+=360; u2g += 360;
}
// Haversine formula for distance // Haversine formula for distance
/** /**
var R = 6371; // km var R = 6371; // km
var dLat = (lat2-lat1).toRad(); var dLat = (lat2-lat1).toRad();
var dLon = (lon2-lon1).toRad(); var dLon = (lon2-lon1).toRad();
var a = Math.sin(dLat/2) * Math.sin(dLat/2) + var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
Math.sin(dLon/2) * Math.sin(dLon/2); Math.sin(dLon/2) * Math.sin(dLon/2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c; var d = R * c;
**/ **/
a = sinf((lat2-lat1)/2) * sinf((lat2-lat1)/2) + a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
cosf(lat1) * cosf(lat2) * c = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
d = 6371 * 1000 * c; d = 6371 * 1000 * c;
// Elevation v depends servo direction // Elevation v depends servo direction
if(d!=0) if (d != 0)
elevation = 90-RAD2DEG(atanf(dAlt/d)); elevation = 90 - RAD2DEG(atanf(dAlt/d));
else else
elevation = 0; elevation = 0;
//! TODO: sanity check //! TODO: sanity check
char temp[50]={0}; char temp[50] =
sprintf(temp,"hea:%d",(int)brng); { 0 };
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); sprintf(temp, "hea:%d", (int) brng);
sprintf(temp,"ele:%d",(int)elevation); write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); sprintf(temp, "ele:%d", (int) elevation);
sprintf(temp,"dis:%d",(int)d); write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); sprintf(temp, "dis:%d", (int) d);
sprintf(temp,"u2g:%d",(int)u2g); write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); sprintf(temp, "u2g:%d", (int) u2g);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"%c%c",(int)(u2g/22.5f)*2+0x90,(int)(u2g/22.5f)*2+0x91); sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91);
write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
} }
int lama=10; int lama = 10;
int lama_loc[2][30]; int lama_loc[2][30];
void lamas(void) void lamas(void)
{ {
char temp[10]={0}; char temp[10] =
lama++; { 0 };
if(lama%10==0) lama++;
{ if (lama % 10 == 0) {
for(int z=0; z<30;z++) for (int z = 0; z < 30; z++) {
{
lama_loc[0][z]=rand()%(GRAPHICS_RIGHT-10); lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10);
lama_loc[1][z]=rand()%(GRAPHICS_BOTTOM-10); lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10);
} }
} }
for(int z=0; z<30;z++) for (int z = 0; z < 30; z++) {
{ sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2));
sprintf(temp,"%c",0xe8+(lama_loc[0][z]%2)); write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
write_string(temp,APPLY_HDEADBAND(lama_loc[0][z]),APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); }
}
} }
//main draw function //main draw function
void updateGraphics() { void updateGraphics()
OsdSettingsData OsdSettings; {
OsdSettingsGet (&OsdSettings); OsdSettingsData OsdSettings;
AttitudeActualData attitude; OsdSettingsGet(&OsdSettings);
AttitudeActualGet(&attitude); AttitudeActualData attitude;
GPSPositionData gpsData; AttitudeActualGet(&attitude);
GPSPositionGet(&gpsData); GPSPositionData gpsData;
HomeLocationData home; GPSPositionGet(&gpsData);
HomeLocationGet(&home); HomeLocationData home;
BaroAltitudeData baro; HomeLocationGet(&home);
BaroAltitudeGet(&baro); BaroAltitudeData baro;
BaroAltitudeGet(&baro);
PIOS_Servo_Set(0,OsdSettings.White); PIOS_Servo_Set(0, OsdSettings.White);
PIOS_Servo_Set(1,OsdSettings.Black); PIOS_Servo_Set(1, OsdSettings.Black);
switch (OsdSettings.Screen) {
case 0: // Dave simple
{
if(home.Set == HOMELOCATION_SET_FALSE)
{
char temps[20]={0};
sprintf(temps,"HOME NOT SET");
//printTextFB(x,y,temp);
write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM/2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
}
switch (OsdSettings.Screen) {
char temp[50]={0}; case 0: // Dave simple
memset(temp, ' ', 40); {
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f); if (home.Set == HOMELOCATION_SET_FALSE) {
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); char temps[20] =
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f); { 0 };
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); sprintf(temps, "HOME NOT SET");
sprintf(temp,"Sat:%d",(int)gpsData.Satellites); //printTextFB(x,y,temp);
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
}
/* Print ADC voltage FLIGHT*/
sprintf(temp,"V:%5.2fV",(PIOS_ADC_PinGet(2)*3*6.1f/4096));
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
if(gpsData.Heading>180)
calcHomeArrow((int16_t)(gpsData.Heading-360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
}
break;
case 1:
{
/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
//write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
//write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
//drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
//drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
//drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
//drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
//drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
/*angleA++;
if(angleB<=-90)
{
sum=2;
}
if(angleB>=90)
{
sum=-2;
}
angleB+=sum;
angleC+=2;*/
// GPS HACK
if(gpsData.Heading>180)
calcHomeArrow((int16_t)(gpsData.Heading-360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
/* Draw Attitude Indicator */
if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED)
{
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96);
}
//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
//printText16( 60, 12,"Hello OP-OSD");
char temp[50]={0};
memset(temp, ' ', 40);
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Fix:%d",(int)gpsData.Status);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
/* Print RTC time */
if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED)
{
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]),APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
}
/* Print Number of detected video Lines */
sprintf(temp,"Lines:%4d",PIOS_Video_GetOSDLines());
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage */
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(5)*3.0f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print CPU temperature */
sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(3)*0.29296875f-264));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage VIDEO*/
sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f*6.1f/4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage RSSI */
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Draw Battery Gauge */
/*m_batt++;
uint8_t dir=3;
if(m_batt==101)
m_batt=0;
if(m_pitch>0)
{
dir=0;
m_alt+=m_pitch/2;
}
else if(m_pitch<0)
{
dir=1;
m_alt+=m_pitch/2;
}*/
/*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) char temp[50] =
{ { 0 };
drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); memset(temp, ' ', 40);
}*/ sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
//drawAltitude(200,50,m_alt,dir); sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
// Draw airspeed (left side.)
if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED)
{
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
}
// Draw altimeter (right side.)
if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED)
{
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
}
// Draw compass.
if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED)
{
if(attitude.Yaw<0) {
hud_draw_linear_compass(360+attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
} else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
}
}
}
break;
case 2:
{
int size=64;
int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2);
draw_artificial_horizon(-attitude.Roll,attitude.Pitch,APPLY_HDEADBAND(x),APPLY_VDEADBAND(y),size);
hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)),
APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
if(OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO)
{
hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
}
else
{
hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
}
} /* Print ADC voltage FLIGHT*/
break; sprintf(temp, "V:%5.2fV", (PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
case 3: write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
{
lamas();
}
break;
case 4:
case 5:
case 6:
{
int image=OsdSettings.Screen-4;
struct splashEntry splash_info;
splash_info = splash[image];
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image); if (gpsData.Heading > 180)
} calcHomeArrow((int16_t)(gpsData.Heading - 360));
break; else
default: calcHomeArrow((int16_t)(gpsData.Heading));
write_vline_lm( APPLY_HDEADBAND(GRAPHICS_RIGHT/2),APPLY_VDEADBAND(0),APPLY_VDEADBAND(GRAPHICS_BOTTOM),1,1); }
write_hline_lm( APPLY_HDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT),APPLY_VDEADBAND(GRAPHICS_BOTTOM/2),1,1); break;
break; case 1:
} {
/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
// Must mask out last half-word because SPI keeps clocking it out otherwise write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
for (uint32_t i = 0; i < 8; i++) { write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
} //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
//drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
//drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
//drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
//drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
//drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
/*angleA++;
if(angleB<=-90)
{
sum=2;
}
if(angleB>=90)
{
sum=-2;
}
angleB+=sum;
angleC+=2;*/
// GPS HACK
if (gpsData.Heading > 180)
calcHomeArrow((int16_t)(gpsData.Heading - 360));
else
calcHomeArrow((int16_t)(gpsData.Heading));
/* Draw Attitude Indicator */
if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96);
}
//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
//printText16( 60, 12,"Hello OP-OSD");
char temp[50] =
{ 0 };
memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Fix:%d", (int) gpsData.Status);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
/* Print RTC time */
if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
}
/* Print Number of detected video Lines */
sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines());
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage */
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
sprintf(temp, "Rssi:%4.2fV", (PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print CPU temperature */
sprintf(temp, "Temp:%4.2fC", (PIOS_ADC_PinGet(3) * 0.29296875f - 264));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage FLIGHT*/
sprintf(temp, "FltV:%4.2fV", (PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage VIDEO*/
sprintf(temp, "VidV:%4.2fV", (PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Print ADC voltage RSSI */
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Draw Battery Gauge */
/*m_batt++;
uint8_t dir=3;
if(m_batt==101)
m_batt=0;
if(m_pitch>0)
{
dir=0;
m_alt+=m_pitch/2;
}
else if(m_pitch<0)
{
dir=1;
m_alt+=m_pitch/2;
}*/
/*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
{
drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
}*/
//drawAltitude(200,50,m_alt,dir);
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.)
if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
}
// Draw altimeter (right side.)
if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
}
// Draw compass.
if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
if (attitude.Yaw < 0) {
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
} else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
}
}
}
break;
case 2:
{
int size = 64;
int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2);
draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size);
hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7,
10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) {
hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
} else {
hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500,
0);
}
}
break;
case 3:
{
lamas();
}
break;
case 4:
case 5:
case 6:
{
int image = OsdSettings.Screen - 4;
struct splashEntry splash_info;
splash_info = splash[image];
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image);
}
break;
default:
write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1);
write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1);
break;
}
// Must mask out last half-word because SPI keeps clocking it out otherwise
for (uint32_t i = 0; i < 8; i++) {
write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
}
} }
void updateOnceEveryFrame() { void updateOnceEveryFrame()
clearGraphics(); {
updateGraphics(); clearGraphics();
updateGraphics();
} }
// **************** // ****************
/** /**
* Initialise the gps module * Initialise the gps module
@ -2431,12 +2318,12 @@ void updateOnceEveryFrame() {
int32_t osdgenStart(void) int32_t osdgenStart(void)
{ {
// Start gps task // Start gps task
vSemaphoreCreateBinary( osdSemaphore); vSemaphoreCreateBinary(osdSemaphore);
xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &osdgenTaskHandle); xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
return 0; return 0;
} }
/** /**
@ -2446,23 +2333,23 @@ int32_t osdgenStart(void)
*/ */
int32_t osdgenInitialize(void) int32_t osdgenInitialize(void)
{ {
AttitudeActualInitialize(); AttitudeActualInitialize();
#ifdef PIOS_INCLUDE_GPS #ifdef PIOS_INCLUDE_GPS
GPSPositionInitialize(); GPSPositionInitialize();
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize(); GPSTimeInitialize();
GPSSatellitesInitialize(); GPSSatellitesInitialize();
#endif #endif
#ifdef PIOS_GPS_SETS_HOMELOCATION #ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationInitialize(); HomeLocationInitialize();
#endif #endif
#endif #endif
OsdSettingsInitialize(); OsdSettingsInitialize();
BaroAltitudeInitialize(); BaroAltitudeInitialize();
return 0; return 0;
} }
MODULE_INITCALL(osdgenInitialize, osdgenStart) MODULE_INITCALL( osdgenInitialize, osdgenStart)
// **************** // ****************
/** /**
@ -2471,49 +2358,42 @@ MODULE_INITCALL(osdgenInitialize, osdgenStart)
static void osdgenTask(void *parameters) static void osdgenTask(void *parameters)
{ {
//portTickType lastSysTime; //portTickType lastSysTime;
// Loop forever // Loop forever
//lastSysTime = xTaskGetTickCount(); //lastSysTime = xTaskGetTickCount();
OsdSettingsData OsdSettings; OsdSettingsData OsdSettings;
OsdSettingsGet (&OsdSettings); OsdSettingsGet(&OsdSettings);
PIOS_Servo_Set(0,OsdSettings.White); PIOS_Servo_Set(0, OsdSettings.White);
PIOS_Servo_Set(1,OsdSettings.Black); PIOS_Servo_Set(1, OsdSettings.Black);
// intro // intro
for(int i=0; i<63; i++) for (int i = 0; i < 63; i++) {
{ if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) clearGraphics();
{ introGraphics();
clearGraphics();
introGraphics();
} }
} }
for(int i=0; i<63; i++) for (int i = 0; i < 63; i++) {
{ if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) clearGraphics();
{ introGraphics();
clearGraphics(); introText();
introGraphics();
introText();
} }
} }
while (1) while (1) {
{ if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) updateOnceEveryFrame();
{
updateOnceEveryFrame();
} }
//xSemaphoreTake(osdSemaphore, portMAX_DELAY); //xSemaphoreTake(osdSemaphore, portMAX_DELAY);
//vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
} }
} }
// **************** // ****************
/** /**
* @} * @}
* @} * @}
*/ */

1025
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -75,7 +75,6 @@
#include "poilocation.h" #include "poilocation.h"
#include "accessorydesired.h" #include "accessorydesired.h"
// Private constants // Private constants
#define MAX_QUEUE_SIZE 4 #define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548 #define STACK_SIZE_BYTES 1548
@ -88,9 +87,9 @@
// Private variables // Private variables
static xTaskHandle pathfollowerTaskHandle; static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static PathStatusData pathStatus; static PathStatusData pathStatus;
static VtolPathFollowerSettingsData vtolpathfollowerSettings; static VtolPathFollowerSettingsData vtolpathfollowerSettings;
static float poiRadius;
// Private functions // Private functions
static void vtolPathFollowerTask(void *parameters); static void vtolPathFollowerTask(void *parameters);
@ -111,13 +110,13 @@ static void accessoryUpdated(UAVObjEvent* ev);
*/ */
int32_t VtolPathFollowerStart() int32_t VtolPathFollowerStart()
{ {
if (vtolpathfollower_enabled) { if (vtolpathfollower_enabled) {
// Start main task // Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); xTaskCreate(vtolPathFollowerTask, (signed char *) "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
} }
return 0; return 0;
} }
/** /**
@ -126,29 +125,29 @@ int32_t VtolPathFollowerStart()
*/ */
int32_t VtolPathFollowerInitialize() int32_t VtolPathFollowerInitialize()
{ {
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules); HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsInitialize();
NedAccelInitialize(); NedAccelInitialize();
PathDesiredInitialize(); PathDesiredInitialize();
PathStatusInitialize(); PathStatusInitialize();
VelocityDesiredInitialize(); VelocityDesiredInitialize();
CameraDesiredInitialize(); CameraDesiredInitialize();
AccessoryDesiredInitialize(); AccessoryDesiredInitialize();
PoiLearnSettingsInitialize(); PoiLearnSettingsInitialize();
PoiLocationInitialize(); PoiLocationInitialize();
vtolpathfollower_enabled = true; vtolpathfollower_enabled = true;
} else { } else {
vtolpathfollower_enabled = false; vtolpathfollower_enabled = false;
} }
return 0; return 0;
} }
MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart) MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart)
static float northVelIntegral = 0; static float northVelIntegral = 0;
static float eastVelIntegral = 0; static float eastVelIntegral = 0;
@ -164,123 +163,117 @@ static float throttleOffset = 0;
*/ */
static void vtolPathFollowerTask(void *parameters) static void vtolPathFollowerTask(void *parameters)
{ {
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
FlightStatusData flightStatus; FlightStatusData flightStatus;
portTickType lastUpdateTime; portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
AccessoryDesiredConnectCallback(accessoryUpdated);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs: VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
// 1. Must have VTOL type airframe AccessoryDesiredConnectCallback(accessoryUpdated);
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings); VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time // Main task loop
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); lastUpdateTime = xTaskGetTickCount();
while (1) {
// Convert the accels into the NED frame // Conditions when this runs:
updateNedAccel(); // 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
FlightStatusGet(&flightStatus); // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
PathStatusGet(&pathStatus);
// Check the combinations of flightmode and pathdesired mode SystemSettingsGet(&systemSettings);
switch(flightStatus.FlightMode) { if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
case FLIGHTSTATUS_FLIGHTMODE_LAND: && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
updateEndpointVelocity(); && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
updateVtolDesiredAttitude(false); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); vTaskDelay(1000);
} else { continue;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); }
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch(pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident // Continue collecting data if not enough time
StabilizationDesiredData stabDesired; vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break; // Convert the accels into the NED frame
} updateNedAccel();
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
} // Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
} }
/** /**
@ -288,49 +281,87 @@ static void vtolPathFollowerTask(void *parameters)
*/ */
static void updatePOIBearing() static void updatePOIBearing()
{ {
PositionActualData positionActual; const float DEADBAND_HIGH = 0.10;
PositionActualGet(&positionActual); const float DEADBAND_LOW = -0.10;
CameraDesiredData cameraDesired; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
//use poi here
//HomeLocationData poi;
//HomeLocationGet (&poi);
float dLoc[3]; PathDesiredData pathDesired;
float yaw=0; PathDesiredGet(&pathDesired);
float elevation=0; PositionActualData positionActual;
PositionActualGet(&positionActual);
CameraDesiredData cameraDesired;
CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
dLoc[0]=positionActual.North-poi.North; float dLoc[3];
dLoc[1]=positionActual.East-poi.East; float yaw = 0;
dLoc[2]=positionActual.Down-poi.Down; float elevation = 0;
if(dLoc[1]<0) dLoc[0] = positionActual.North - poi.North;
yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))+180; dLoc[1] = positionActual.East - poi.East;
else dLoc[2] = positionActual.Down - poi.Down;
yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))-180;
// distance if (dLoc[1] < 0) {
float distance = sqrtf(powf(dLoc[0],2)+powf(dLoc[1],2)); yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180;
} else {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
}
//not above // distance
if(distance!=0) { float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
//You can feed this into camerastabilization
elevation=RAD2DEG(atan2f(dLoc[2],distance));
}
stabDesired.Yaw=yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
cameraDesired.Yaw=yaw;
cameraDesired.PitchOrServo2=elevation;
CameraDesiredSet(&cameraDesired); ManualControlCommandData manualControlData;
StabilizationDesiredSet(&stabDesired); ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) {
if (pathAngle != 0 || changeRadius != 0) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf((pathAngle + yaw - 180.0f) * DEG2RAD));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf((pathAngle + yaw - 180.0f) * DEG2RAD));
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
//not above
if (distance >= 3.0f) {
//You can feed this into camerastabilization
elevation = RAD2DEG(atan2f(dLoc[2],distance));
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
//cameraDesired.Yaw=yaw;
//cameraDesired.PitchOrServo2=elevation;
CameraDesiredSet(&cameraDesired);
StabilizationDesiredSet(&stabDesired);
}
} }
/** /**
* Compute desired velocity from the current position and path * Compute desired velocity from the current position and path
* *
@ -339,75 +370,74 @@ static void updatePOIBearing()
*/ */
static void updatePathVelocity() static void updatePathVelocity()
{ {
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand; float downCommand;
PositionActualData positionActual; PathDesiredData pathDesired;
PositionActualGet(&positionActual); PathDesiredGet(&pathDesired);
PositionActualData positionActual;
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; PositionActualGet(&positionActual);
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed; float cur[3] =
switch (pathDesired.Mode) { { positionActual.North, positionActual.East, positionActual.Down };
case PATHDESIRED_MODE_FLYCIRCLERIGHT: struct path_status progress;
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity *
bound(progress.fractional_progress,0,1);
if(progress.fractional_progress > 1)
groundspeed = 0;
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1);
if(progress.fractional_progress > 1)
groundspeed = 0;
break;
}
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
float scale = 1;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North += progress.correction_direction[0] * error_speed * scale; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
float downError = altitudeSetpoint - positionActual.Down; float groundspeed;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], switch (pathDesired.Mode) {
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], case PATHDESIRED_MODE_FLYCIRCLERIGHT:
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); case PATHDESIRED_MODE_FLYCIRCLELEFT:
velocityDesired.Down = bound(downCommand, case PATHDESIRED_MODE_DRIVECIRCLELEFT:
-vtolpathfollowerSettings.VerticalVelMax, groundspeed = pathDesired.EndingVelocity;
vtolpathfollowerSettings.VerticalVelMax); break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1)
groundspeed = 0;
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1)
groundspeed = 0;
break;
}
// update pathstatus VelocityDesiredData velocityDesired;
pathStatus.error = progress.error; velocityDesired.North = progress.path_direction[0] * groundspeed;
pathStatus.fractional_progress = progress.fractional_progress; velocityDesired.East = progress.path_direction[1] * groundspeed;
VelocityDesiredSet(&velocityDesired); float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] =
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
} }
/** /**
@ -418,89 +448,88 @@ static void updatePathVelocity()
*/ */
void updateEndpointVelocity() void updateEndpointVelocity()
{ {
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PositionActualData positionActual; PathDesiredData pathDesired;
VelocityDesiredData velocityDesired; PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
VelocityDesiredGet(&velocityDesired);
float northError; PositionActualData positionActual;
float eastError; VelocityDesiredData velocityDesired;
float downError;
float northCommand;
float eastCommand;
float downCommand;
float northPos = 0, eastPos = 0, downPos = 0;
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
float alt = homeLocation.Altitude;
float T[3] = { alt+6.378137E6f,
cosf(lat)*(alt+6.378137E6f),
-1.0f};
float NED[3] = {T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD),
T[1] * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD),
T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))};
northPos = NED[0]; PositionActualGet(&positionActual);
eastPos = NED[1]; VelocityDesiredGet(&velocityDesired);
downPos = NED[2];
}
break;
default:
PIOS_Assert(0);
break;
}
// Compute desired north command float northError;
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; float eastError;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], float downError;
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], float northCommand;
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); float eastCommand;
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + float downCommand;
northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
float scale = 1;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale; float northPos = 0, eastPos = 0, downPos = 0;
velocityDesired.East = eastCommand * scale; switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
float alt = homeLocation.Altitude;
float T[3] =
{ alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f };
float NED[3] =
{ T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1]
* ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2]
* ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; northPos = NED[0];
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], eastPos = NED[1];
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], downPos = NED[2];
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); }
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); break;
velocityDesired.Down = bound(downCommand, default:
-vtolpathfollowerSettings.VerticalVelMax, PIOS_Assert(0);
vtolpathfollowerSettings.VerticalVelMax); break;
}
VelocityDesiredSet(&velocityDesired);
// Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
float scale = 1;
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
} }
/** /**
@ -509,16 +538,16 @@ void updateEndpointVelocity()
*/ */
static void updateFixedAttitude(float* attitude) static void updateFixedAttitude(float* attitude)
{ {
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0]; stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1]; stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2]; stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3]; stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }
/** /**
@ -530,129 +559,124 @@ static void updateFixedAttitude(float* attitude)
*/ */
static void updateVtolDesiredAttitude(bool yaw_attitude) static void updateVtolDesiredAttitude(bool yaw_attitude)
{ {
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
VelocityActualData velocityActual; VelocityActualData velocityActual;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
NedAccelData nedAccel; NedAccelData nedAccel;
VtolPathFollowerSettingsData vtolpathfollowerSettings; VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
float northError; float northError;
float northCommand; float northCommand;
float eastError;
float eastCommand;
float downError; float eastError;
float downCommand; float eastCommand;
SystemSettingsGet(&systemSettings); float downError;
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); float downCommand;
VelocityActualGet(&velocityActual); SystemSettingsGet(&systemSettings);
VelocityDesiredGet(&velocityDesired); VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired); VelocityActualGet(&velocityActual);
AttitudeActualGet(&attitudeActual); VelocityDesiredGet(&velocityDesired);
StabilizationSettingsGet(&stabSettings); StabilizationDesiredGet(&stabDesired);
NedAccelGet(&nedAccel); VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
float northVel = 0, eastVel = 0, downVel = 0; StabilizationSettingsGet(&stabSettings);
switch (vtolpathfollowerSettings.VelocitySource) { NedAccelGet(&nedAccel);
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North; float northVel = 0, eastVel = 0, downVel = 0;
eastVel = velocityActual.East; switch (vtolpathfollowerSettings.VelocitySource) {
downVel = velocityActual.Down; case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
break; northVel = velocityActual.North;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: eastVel = velocityActual.East;
{ downVel = velocityActual.Down;
GPSVelocityData gpsVelocity; break;
GPSVelocityGet(&gpsVelocity); case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
northVel = gpsVelocity.North; {
eastVel = gpsVelocity.East; GPSVelocityData gpsVelocity;
downVel = gpsVelocity.Down; GPSVelocityGet(&gpsVelocity);
} northVel = gpsVelocity.North;
break; eastVel = gpsVelocity.East;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: downVel = gpsVelocity.Down;
{ }
GPSPositionData gpsPosition; break;
GPSPositionGet(&gpsPosition); case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); {
eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); GPSPositionData gpsPosition;
downVel = velocityActual.Down; GPSPositionGet(&gpsPosition);
} northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
break; eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
default: downVel = velocityActual.Down;
PIOS_Assert(0); }
break; break;
} default:
PIOS_Assert(0);
// Testing code - refactor into manual control command break;
ManualControlCommandData manualControlData; }
ManualControlCommandGet(&manualControlData);
// Testing code - refactor into manual control command
// Compute desired north command ManualControlCommandData manualControlData;
northError = velocityDesired.North - northVel; ManualControlCommandGet(&manualControlData);
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], // Compute desired north command
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); northError = velocityDesired.North - northVel;
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
northVelIntegral - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
// Compute desired east command + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Compute desired east command
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], eastError = velocityDesired.East - eastVel;
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
eastVelIntegral - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel; // Compute desired down command
// Must flip this sign downError = velocityDesired.Down - downVel;
downError = -downError; // Must flip this sign
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], downError = -downError;
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downVelIntegral - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch // craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandData manualControl;
// For now override throttle with manual control. Disable at your risk, quad goes to China. ManualControlCommandGet(&manualControl);
ManualControlCommandData manualControl; stabDesired.Throttle = manualControl.Throttle;
ManualControlCommandGet(&manualControl); }
stabDesired.Throttle = manualControl.Throttle;
} stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if(yaw_attitude) { } else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
} else { stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; }
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; StabilizationDesiredSet(&stabDesired);
}
StabilizationDesiredSet(&stabDesired);
} }
/** /**
@ -660,39 +684,39 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
*/ */
static void updateNedAccel() static void updateNedAccel()
{ {
float accel[3]; float accel[3];
float q[4]; float q[4];
float Rbe[3][3]; float Rbe[3][3];
float accel_ned[3]; float accel_ned[3];
// Collect downsampled attitude data // Collect downsampled attitude data
AccelsData accels; AccelsData accels;
AccelsGet(&accels); AccelsGet(&accels);
accel[0] = accels.x; accel[0] = accels.x;
accel[1] = accels.y; accel[1] = accels.y;
accel[2] = accels.z; accel[2] = accels.z;
//rotate avg accels into earth frame and store it //rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
q[0]=attitudeActual.q1; q[0] = attitudeActual.q1;
q[1]=attitudeActual.q2; q[1] = attitudeActual.q2;
q[2]=attitudeActual.q3; q[2] = attitudeActual.q3;
q[3]=attitudeActual.q4; q[3] = attitudeActual.q4;
Quaternion2R(q, Rbe); Quaternion2R(q, Rbe);
for (uint8_t i=0; i<3; i++){ for (uint8_t i = 0; i < 3; i++) {
accel_ned[i]=0; accel_ned[i] = 0;
for (uint8_t j=0; j<3; j++) for (uint8_t j = 0; j < 3; j++)
accel_ned[i] += Rbe[j][i]*accel[j]; accel_ned[i] += Rbe[j][i] * accel[j];
} }
accel_ned[2] += 9.81f; accel_ned[2] += 9.81f;
NedAccelData accelData; NedAccelData accelData;
NedAccelGet(&accelData); NedAccelGet(&accelData);
accelData.North = accel_ned[0]; accelData.North = accel_ned[0];
accelData.East = accel_ned[1]; accelData.East = accel_ned[1];
accelData.Down = accel_ned[2]; accelData.Down = accel_ned[2];
NedAccelSet(&accelData); NedAccelSet(&accelData);
} }
/** /**
@ -700,42 +724,41 @@ static void updateNedAccel()
*/ */
static float bound(float val, float min, float max) static float bound(float val, float min, float max)
{ {
if (val < min) { if (val < min) {
val = min; val = min;
} else if (val > max) { } else if (val > max) {
val = max; val = max;
} }
return val; return val;
} }
static void SettingsUpdatedCb(UAVObjEvent * ev) static void SettingsUpdatedCb(UAVObjEvent * ev)
{ {
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
} }
static void accessoryUpdated(UAVObjEvent* ev) static void accessoryUpdated(UAVObjEvent* ev)
{ {
if (ev->obj != AccessoryDesiredHandle()) if (ev->obj != AccessoryDesiredHandle())
return; return;
PositionActualData positionActual; AccessoryDesiredData accessory;
PositionActualGet(&positionActual); PoiLearnSettingsData poiLearn;
AccessoryDesiredData accessory; PoiLearnSettingsGet(&poiLearn);
PoiLearnSettingsData poiLearn;
PoiLearnSettingsGet(&poiLearn); if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
PoiLocationData poi; if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
PoiLocationGet(&poi); if (accessory.AccessoryVal < -0.5f) {
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { PositionActualData positionActual;
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { PositionActualGet(&positionActual);
if(accessory.AccessoryVal<-0.5f) PoiLocationData poi;
{ PoiLocationGet(&poi);
poi.North=positionActual.North; poi.North = positionActual.North;
poi.East=positionActual.East; poi.East = positionActual.East;
poi.Down=positionActual.Down; poi.Down = positionActual.Down;
PoiLocationSet(&poi); PoiLocationSet(&poi);
} }
} }
} }
} }

51
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
View File

@ -49,10 +49,11 @@ bool AeroSimRCSimulator::setupProcess()
void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
{ {
Q_UNUSED(outPort) Q_UNUSED(outPort)
if (inSocket->bind(QHostAddress(host), inPort)) if (inSocket->bind(QHostAddress(host), inPort)) {
emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n"); emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
else } else {
emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n"); emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
}
} }
void AeroSimRCSimulator::transmitUpdate() void AeroSimRCSimulator::transmitUpdate()
@ -79,20 +80,16 @@ void AeroSimRCSimulator::transmitUpdate()
float yaw = -1; float yaw = -1;
float throttle = -1; float throttle = -1;
if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
{
// Read joystick input // Read joystick input
if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
{
// Note: Pitch sign is reversed in FG ? // Note: Pitch sign is reversed in FG ?
roll = manCtrlData.Roll; roll = manCtrlData.Roll;
pitch = -manCtrlData.Pitch; pitch = -manCtrlData.Pitch;
yaw = manCtrlData.Yaw; yaw = manCtrlData.Yaw;
throttle = manCtrlData.Throttle; throttle = manCtrlData.Throttle;
} }
} } else {
else
{
// Read ActuatorDesired from autopilot // Read ActuatorDesired from autopilot
actData = actDesired->getData(); actData = actDesired->getData();
@ -101,12 +98,13 @@ void AeroSimRCSimulator::transmitUpdate()
yaw = actData.Yaw; yaw = actData.Yaw;
throttle = (actData.Throttle*2.0)-1.0; throttle = (actData.Throttle*2.0)-1.0;
} }
channels[0]=roll; channels[0] = roll;
channels[1]=pitch; channels[1] = pitch;
if(throttle<-1) if(throttle < -1) {
throttle=-1; throttle = -1;
channels[2]=throttle; }
channels[3]=yaw; channels[2] = throttle;
channels[3] = yaw;
// read flight status // read flight status
quint8 armed; quint8 armed;
@ -120,8 +118,9 @@ void AeroSimRCSimulator::transmitUpdate()
QDataStream stream(&data, QIODevice::WriteOnly); QDataStream stream(&data, QIODevice::WriteOnly);
stream.setFloatingPointPrecision(QDataStream::SinglePrecision); stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
stream << quint32(0x52434D44); // magic header, "RCMD" stream << quint32(0x52434D44); // magic header, "RCMD"
for (int i = 0; i < 10; ++i) for (int i = 0; i < 10; ++i) {
stream << channels[i]; // channels stream << channels[i]; // channels
}
stream << armed << mode; // flight status stream << armed << mode; // flight status
stream << udpCounterASrecv; // packet counter stream << udpCounterASrecv; // packet counter
@ -193,10 +192,10 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
memset(&out, 0, sizeof(Output2Hardware)); memset(&out, 0, sizeof(Output2Hardware));
out.delT=delT; out.delT = delT;
/*************************************************************************************/ /*************************************************************************************/
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){ for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) {
out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1 out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1
} }
@ -245,22 +244,20 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
out.groundspeed = qSqrt(velX * velX + velY * velY); out.groundspeed = qSqrt(velX * velX + velY * velY);
/**********************************************************************************************/ /**********************************************************************************************/
out.dstN = posY * 1; out.dstN = posY;
out.dstE = posX * 1; out.dstE = posX;
out.dstD = posZ * -1; out.dstD = -posZ;
out.velNorth = velY * 1; out.velNorth = velY;
out.velEast = velX * 1; out.velEast = velX;
out.velDown = velZ * -1; out.velDown = -velZ;
out.voltage = volt; out.voltage = volt;
out.current = curr; out.current = curr;
out.consumption = cons*1000.0; out.consumption = cons * 1000.0;
updateUAVOs(out); updateUAVOs(out);
#ifdef DBG_TIMERS #ifdef DBG_TIMERS
static int cntRX = 0; static int cntRX = 0;
if (cntRX >= 100) { if (cntRX >= 100) {

2
make/common-defs.mk
View File

@ -130,7 +130,7 @@ endif
ifeq ($(DEBUG), YES) ifeq ($(DEBUG), YES)
CFLAGS += -DDEBUG CFLAGS += -DDEBUG
else else
CFLAGS += -fdata-sections -ffunction-sections CFLAGS += -ffunction-sections #-fdata-sections
endif endif
# Compiler flags to generate dependency files # Compiler flags to generate dependency files