rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-11 01:54:14 +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

16
flight/Libraries/paths.c
View File

@ -211,27 +211,29 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
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) if (a_diff < 0) {
a_diff += 2 * M_PI; a_diff += 2 * M_PI;
if(a_radius<0) }
if (a_radius < 0) {
a_radius += 2 * M_PI; a_radius += 2 * M_PI;
}
progress = (a_diff - a_radius + M_PI) / (2 * M_PI); progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
if(progress<0) if (progress < 0) {
progress += 1.0; progress += 1.0;
else if(progress>=1) } else if (progress >= 1) {
progress -= 1.0; progress -= 1.0;
}
if(clockwise) if (clockwise) {
{
progress = 1 - progress; progress = 1 - progress;
} }
status->fractional_progress = progress; status->fractional_progress = progress;
// error is current radius minus wanted radius - positive if too close // error is current radius minus wanted radius - positive if too close

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

@ -149,11 +149,10 @@ 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
@ -164,8 +163,7 @@ static void altitudeTask(void *parameters)
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();

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

@ -110,13 +110,11 @@ 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
@ -127,8 +125,7 @@ static void altitudeTask(void *parameters)
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();

210
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,7 +110,8 @@ 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; ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count; uint8_t sample_count;
@ -238,16 +235,13 @@ static void manualControlTask(void *parameters)
bool valid_input_detected = true; bool valid_input_detected = true;
// Read channel values in us // Read channel values in us
for (uint8_t n = 0; for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
++n) {
extern uint32_t pios_rcvr_group_map[]; extern uint32_t pios_rcvr_group_map[];
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID; cmd.Channel[n] = PIOS_RCVR_INVALID;
} else { } else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]);
settings.ChannelNumber[n]);
} }
// If a channel has timed out this is not valid data and we shouldn't update anything // If a channel has timed out this is not valid data and we shouldn't update anything
@ -259,27 +253,29 @@ static void manualControlTask(void *parameters)
} }
// Check settings, if error raise alarm // Check settings, if error raise alarm
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid // Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (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_YAW] == (uint16_t) PIOS_RCVR_INVALID
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID || || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID
||
// Check the driver exists // Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (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_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid // Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || 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 // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) && ( ((settings.FlightModeNumber > 1)
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || && (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_INVALID
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
@ -293,10 +289,14 @@ static void manualControlTask(void *parameters)
} }
// decide if we have valid manual input or not // decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& 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]);
// Implement hysteresis loop on connection status // Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) { if (valid_input_detected && (++connected_count > 10)) {
@ -322,22 +322,19 @@ static void manualControlTask(void *parameters)
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) if (AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) if (AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) if (AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -371,15 +368,14 @@ static void manualControlTask(void *parameters)
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; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
@ -388,8 +384,7 @@ static void manualControlTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
@ -398,8 +393,7 @@ static void manualControlTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
@ -484,8 +478,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
/* 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;
@ -499,7 +492,8 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
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++) { for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed // Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
@ -511,9 +505,7 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_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;
channel++) {
uint16_t delta; uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
@ -580,9 +572,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
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,
NELEMENTS(fsm->prev));
fsm->sample_count++; fsm->sample_count++;
return (false); return (false);
} }
@ -608,9 +598,7 @@ group_completed:
* 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,
NELEMENTS(fsm->prev));
fsm->sample_count++; fsm->sample_count++;
break; break;
} }
@ -662,35 +650,41 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
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_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? 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_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? 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_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
0; // this is an invalid mode (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
; ;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? 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_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (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_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
0; // this is an invalid mode (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_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? 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_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? 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_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
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);
@ -755,7 +749,6 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
} }
} }
static void updateLandDesired(ManualControlCommandData * cmd, bool changed) static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
{ {
static portTickType lastSysTime; static portTickType lastSysTime;
@ -821,12 +814,14 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool 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
altitudeHoldDesired.Altitude = 0; altitudeHoldDesired.Altitude = 0;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
else if (cmd->Throttle < DEADBAND_LOW && zeroed) } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT; 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 } 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; zeroed = true;
}
AltitudeHoldDesiredSet(&altitudeHoldDesired); AltitudeHoldDesiredSet(&altitudeHoldDesired);
} }
@ -858,32 +853,35 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
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) {
if (min != neutral)
valueScaled = (float) (value - neutral) / (float) (neutral - min); valueScaled = (float) (value - neutral) / (float) (neutral - min);
else } else {
valueScaled = 0; 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) {
if (end_time > start_time) {
return (end_time - start_time) * portTICK_RATE_MS; return (end_time - start_time) * portTICK_RATE_MS;
}
return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS; return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS;
} }
@ -897,12 +895,9 @@ static bool okToArm(void)
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
// Check each alarm // Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
{ if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue; continue;
@ -932,7 +927,8 @@ static bool forcedDisArm(void)
* @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; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
@ -990,7 +986,6 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
return; return;
} }
// When the configuration is not "Always armed" and no "Always disarmed", // When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low // the state will not be changed when the throttle is not low
static portTickType armedDisarmStart; static portTickType armedDisarmStart;
@ -999,9 +994,15 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
// Calc channel see assumptions7 // Calc channel see assumptions7
int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1; int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) { switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; case ARMING_CHANNEL_ROLL:
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; armingInputLevel = sign * cmd->Roll;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; break;
case ARMING_CHANNEL_PITCH:
armingInputLevel = sign * cmd->Pitch;
break;
case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw;
break;
} }
bool manualArm = false; bool manualArm = false;
@ -1092,8 +1093,7 @@ 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;
@ -1108,8 +1108,7 @@ 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;
@ -1128,7 +1127,6 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
} }
} }
#endif // USE_INPUT_LPF #endif // USE_INPUT_LPF
/** /**
* Called whenever a critical configuration component changes * Called whenever a critical configuration component changes
*/ */
@ -1137,8 +1135,6 @@ static void configurationUpdatedCb(UAVObjEvent * ev)
configuration_check(); configuration_check();
} }
/** /**
* @} * @}
* @} * @}

796
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"
@ -69,7 +68,6 @@ 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;
// **************** // ****************
@ -99,42 +97,32 @@ struct splashEntry
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_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset((uint8_t *) draw_buffer_level, 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 //check top/left position
if (!validPos(offsetx, offsety)) { if (!validPos(offsetx, offsety)) {
return; return;
@ -145,16 +133,20 @@ void copyimage(uint16_t offsetx, uint16_t offsety, int image) {
for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) { for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
uint16_t x1 = offsetx; uint16_t x1 = offsetx;
for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) { for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8); draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF); mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
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_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
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); draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
x1 += 2; 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) { if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
return 0; return 0;
} }
@ -162,7 +154,8 @@ uint8_t validPos(uint16_t x, uint16_t y) {
} }
// 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 f = 1 - radius;
int ddF_x = 1; int ddF_x = 1;
int ddF_y = -2 * radius; int ddF_y = -2 * radius;
@ -174,13 +167,11 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
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--; y--;
ddF_y += 2; ddF_y += 2;
f += ddF_y; f += ddF_y;
@ -199,21 +190,20 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
} }
} }
void swap(uint16_t* a, uint16_t* b) { void swap(uint16_t* a, uint16_t* b)
{
uint16_t temp = *a; uint16_t temp = *a;
*a = *b; *a = *b;
*b = temp; *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,
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};
static int8_t mySin(uint16_t angle) {
uint16_t pos = 0; uint16_t pos = 0;
pos = angle % 360; pos = angle % 360;
int8_t mult = 1; int8_t mult = 1;
@ -229,7 +219,8 @@ static int8_t mySin(uint16_t angle) {
return mult * (int8_t)(sinData[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);
} }
@ -275,15 +266,13 @@ 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);
@ -292,17 +281,16 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
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;
@ -312,7 +300,6 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
} }
} }
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);
@ -379,7 +366,6 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
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
* *
@ -393,11 +379,12 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
{ {
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) return; if (x0 == x1) {
return;
}
/* This is an optimised algorithm for writing horizontal lines. /* This is an optimised algorithm for writing horizontal lines.
* We begin by finding the addresses of the x0 and x1 points. */ * We begin by finding the addresses of the x0 and x1 points. */
int addr0 = CALC_BUFF_ADDR(x0, y); int addr0 = CALC_BUFF_ADDR(x0, y);
@ -407,21 +394,17 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
int mask, mask_l, mask_r, i; int mask, mask_l, mask_r, i;
/* If the addresses are equal, we only need to write one word /* If the addresses are equal, we only need to write one word
* which is an island. */ * which is an island. */
if(addr0 == addr1) 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_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
WRITE_WORD_MODE(buff, addr0, mask_l, mode); WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode); WRITE_WORD_MODE(buff, addr1, mask_r, mode);
// Now write 0xffff words from start+1 to end-1. // Now write 0xffff words from start+1 to end-1.
for(i = addr0 + 1; i <= addr1 - 1; i++) for (i = addr0 + 1; i <= addr1 - 1; i++) {
{
uint8_t m = 0xff; uint8_t m = 0xff;
WRITE_WORD_MODE(buff, i, m, mode); WRITE_WORD_MODE(buff, i, m, mode);
} }
@ -461,8 +444,7 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
{ {
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.
@ -488,11 +470,12 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
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) return; if (y0 == y1) {
return;
}
/* This is an optimised algorithm for writing vertical lines. /* This is an optimised algorithm for writing vertical lines.
* We begin by finding the addresses of the x,y0 and x,y1 points. */ * We begin by finding the addresses of the x,y0 and x,y1 points. */
int addr0 = CALC_BUFF_ADDR(x, y0); int addr0 = CALC_BUFF_ADDR(x, y0);
@ -502,8 +485,7 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
uint16_t mask = 1 << (7 - bitnum); uint16_t mask = 1 << (7 - bitnum);
/* Run from addr0 to addr1 placing pixels. Increment by the number /* Run from addr0 to addr1 placing pixels. Increment by the number
* of words n each graphics line. */ * of words n each graphics line. */
for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
{
WRITE_WORD_MODE(buff, a, mask, mode); WRITE_WORD_MODE(buff, a, mask, mode);
} }
} }
@ -540,8 +522,7 @@ 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);
@ -574,7 +555,9 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
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) {
return;
}
// Calculate as if the rectangle was only a horizontal line. We then // Calculate as if the rectangle was only a horizontal line. We then
// step these addresses through each row until we iterate `height` times. // step these addresses through each row until we iterate `height` times.
int addr0 = CALC_BUFF_ADDR(x, y); int addr0 = CALC_BUFF_ADDR(x, y);
@ -583,26 +566,21 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
int addr1_bit = CALC_BIT_IN_WORD(x + width); int addr1_bit = CALC_BIT_IN_WORD(x + width);
int mask, mask_l, mask_r, i; int mask, mask_l, mask_r, i;
// If the addresses are equal, we need to write one word vertically. // If the addresses are equal, we need to write one word vertically.
if(addr0 == addr1) if (addr0 == addr1) {
{
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
while(height--) 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_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
// Write edges first. // Write edges first.
yy = 0; yy = 0;
addr0_old = addr0; addr0_old = addr0;
addr1_old = addr1; addr1_old = addr1;
while(yy < height) while (yy < height) {
{
WRITE_WORD_MODE(buff, addr0, mask_l, mode); WRITE_WORD_MODE(buff, addr0, mask_l, mode);
WRITE_WORD_MODE(buff, addr1, mask_r, mode); WRITE_WORD_MODE(buff, addr1, mask_r, mode);
addr0 += GRAPHICS_WIDTH_REAL / 8; addr0 += GRAPHICS_WIDTH_REAL / 8;
@ -613,10 +591,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
yy = 0; yy = 0;
addr0 = addr0_old; addr0 = addr0_old;
addr1 = addr1_old; addr1 = addr1_old;
while(yy < height) while (yy < height) {
{ for (i = addr0 + 1; i <= addr1 - 1; i++) {
for(i = addr0 + 1; i <= addr1 - 1; i++)
{
uint8_t m = 0xff; uint8_t m = 0xff;
WRITE_WORD_MODE(buff, i, m, mode); WRITE_WORD_MODE(buff, i, m, mode);
} }
@ -681,16 +657,13 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
{ {
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; error += (y * 2) + 1;
y++; y++;
if(error >= 0) if (error >= 0) {
{
--x; --x;
error -= x * 2; error -= x * 2;
} }
@ -716,10 +689,8 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
// 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_mask, cx, cy, x + 1, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
@ -728,8 +699,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
if(bmode == 1) if (bmode == 1) {
{
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_mask, cx, cy, x - 1, y - 1, mmode);
@ -738,8 +708,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
} }
error += (y * 2) + 1; error += (y * 2) + 1;
y++; y++;
if(error >= 0) if (error >= 0) {
{
--x; --x;
error -= x * 2; error -= x * 2;
} }
@ -747,17 +716,14 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
error = -r; error = -r;
x = r; x = r;
y = 0; 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, y, mmode); CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill); CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
} }
error += (y * 2) + 1; error += (y * 2) + 1;
y++; y++;
if(error >= 0) if (error >= 0) {
{
--x; --x;
error -= x * 2; error -= x * 2;
} }
@ -784,14 +750,11 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
// //
// 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); write_hline(buff, cx - x, cx + x, cy - y, mode);
if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
{
write_hline(buff, cx - y, cx + y, cy + x, mode); write_hline(buff, cx - y, cx + y, cy + x, mode);
write_hline(buff, cx - y, cx + y, cy - x, mode); write_hline(buff, cx - y, cx + y, cy - x, mode);
xch = 0; xch = 0;
@ -799,16 +762,14 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
} }
error += (y * 2) + 1; error += (y * 2) + 1;
y++; y++;
if(error >= 0) if (error >= 0) {
{
--x; --x;
xch = 1; xch = 1;
error -= x * 2; error -= x * 2;
} }
} }
// Handle toggle mode. // Handle toggle mode.
if(mode == 2) if (mode == 2) {
{
write_hline(buff, cx - r, cx + r, cy, mode); write_hline(buff, cx - r, cx + r, cy, mode);
} }
} }
@ -827,13 +788,11 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
{ {
// 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(x0, x1);
SWAP(y0, y1); SWAP(y0, y1);
} }
@ -843,23 +802,19 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
int ystep; int ystep;
int y = y0; int y = y0;
int x; //, lasty = y, stox = 0; int x; //, lasty = y, stox = 0;
if(y0 < y1) if (y0 < y1) {
ystep = 1; ystep = 1;
else } else {
ystep = -1; ystep = -1;
for(x = x0; x < x1; x++)
{
if(steep)
{
write_pixel(buff, y, x, mode);
} }
else for (x = x0; x < x1; x++) {
{ if (steep) {
write_pixel(buff, y, x, mode);
} else {
write_pixel(buff, x, y, mode); write_pixel(buff, x, y, mode);
} }
error -= deltay; error -= deltay;
if(error < 0) if (error < 0) {
{
y += ystep; y += ystep;
error += deltax; error += deltax;
} }
@ -900,24 +855,19 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
// 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 {
else
{
omode = 1; omode = 1;
imode = 0; imode = 0;
} }
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(x0, x1);
SWAP(y0, y1); SWAP(y0, y1);
} }
@ -927,30 +877,26 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
int ystep; int ystep;
int y = y0; int y = y0;
int x; int x;
if(y0 < y1) if (y0 < y1) {
ystep = 1; ystep = 1;
else } else {
ystep = -1; ystep = -1;
}
// Draw the outline. // Draw the outline.
for(x = x0; x < x1; x++) for (x = x0; x < x1; x++) {
{ if (steep) {
if(steep)
{
write_pixel_lm(y - 1, x, mmode, omode); write_pixel_lm(y - 1, x, mmode, omode);
write_pixel_lm(y + 1, x, mmode, omode); write_pixel_lm(y + 1, x, mmode, omode);
write_pixel_lm(y, x - 1, mmode, omode); write_pixel_lm(y, x - 1, mmode, omode);
write_pixel_lm(y, x + 1, mmode, omode); write_pixel_lm(y, x + 1, mmode, omode);
} } else {
else
{
write_pixel_lm(x - 1, y, mmode, omode); write_pixel_lm(x - 1, y, mmode, omode);
write_pixel_lm(x + 1, y, mmode, omode); write_pixel_lm(x + 1, y, mmode, omode);
write_pixel_lm(x, y - 1, mmode, omode); write_pixel_lm(x, y - 1, mmode, omode);
write_pixel_lm(x, y + 1, mmode, omode); write_pixel_lm(x, y + 1, mmode, omode);
} }
error -= deltay; error -= deltay;
if(error < 0) if (error < 0) {
{
y += ystep; y += ystep;
error += deltax; error += deltax;
} }
@ -958,19 +904,14 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
// Now draw the innards. // Now draw the innards.
error = deltax / 2; error = deltax / 2;
y = y0; y = y0;
for(x = x0; x < x1; x++) for (x = x0; x < x1; x++) {
{ if (steep) {
if(steep)
{
write_pixel_lm(y, x, mmode, imode); write_pixel_lm(y, x, mmode, imode);
} } else {
else
{
write_pixel_lm(x, y, mmode, imode); write_pixel_lm(x, y, mmode, imode);
} }
error -= deltay; error -= deltay;
if(error < 0) if (error < 0) {
{
y += ystep; y += ystep;
error += deltax; error += deltax;
} }
@ -995,9 +936,10 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
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);
} }
}
/** /**
* write_word_misaligned_NAND: Write a misaligned word across two addresses * write_word_misaligned_NAND: Write a misaligned word across two addresses
@ -1020,9 +962,10 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
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);
} }
}
/** /**
* write_word_misaligned_OR: Write a misaligned word across two addresses * write_word_misaligned_OR: Write a misaligned word across two addresses
@ -1045,8 +988,9 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
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);
}
} }
@ -1077,21 +1021,21 @@ 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. // Load the font info; IDs are always sequential.
*font_info = fonts[font]; *font_info = fonts[font];
// Locate character in font lookup table. (If required.) // Locate character in font lookup table. (If required.)
if(lookup != NULL) 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
@ -1120,20 +1064,21 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
// 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. // Load data pointer.
row = ch * font_info.height; row = ch * font_info.height;
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++) {
{ 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; addr += GRAPHICS_WIDTH_REAL / 8;
row++; row++;
} }
@ -1143,10 +1088,8 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
// 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++) {
{ 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;
@ -1169,8 +1112,6 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
} }
} }
/** /**
* 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
@ -1201,19 +1142,18 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
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++;
@ -1224,11 +1164,12 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
// 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]; level_bits = font_info.data[row + font_info.height];
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_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);
@ -1255,20 +1196,20 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
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) {
if(line_length > max_length)
max_length = line_length; max_length = line_length;
}
line_length = 0; line_length = 0;
lines++; lines++;
} }
str++; str++;
} }
if(line_length > max_length) if (line_length > max_length) {
max_length = line_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);
} }
@ -1295,36 +1236,42 @@ 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; yy += ys + font_info.height;
xx = xx_original; xx = xx_original;
} } else {
else if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
{ if (font_info.id < 2) {
if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL)
{
if(font_info.id<2)
write_char(*str, xx, yy, flags, font); write_char(*str, xx, yy, flags, font);
else } else {
write_char16(*str, xx, yy, font); write_char16(*str, xx, yy, font);
} }
}
xx += font_info.width + xs; xx += font_info.width + xs;
} }
str++; str++;
@ -1360,53 +1307,51 @@ 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) {
// begin format code?
fcode = 1; fcode = 1;
fptr = 0; 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;
} } 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;
} }
if(fheight > max_height) if (fheight > max_height) {
max_height = fheight; max_height = fheight;
}
// Skip over this byte. Go to next byte. // Skip over this byte. Go to next byte.
str++; str++;
continue; continue;
} }
if(*str != '<' && *str != '>' && fcode == 1) if (*str != '<' && *str != '>' && fcode == 1) {
{
// Add to the format stack (up to 10 bytes.) // Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes if (fptr > 10) {
{ // stop adding bytes
str++; // go to next byte str++; // go to next byte
continue; continue;
} }
fstack[fptr++] = *str; fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
} }
if(fcode == 0) if (fcode == 0) {
{
// Not a format code, raw text. // Not a format code, raw text.
xx += fwidth + xs; xx += fwidth + xs;
if(*str == '\n') if (*str == '\n') {
{ if (xx > max_xx) {
if(xx > max_xx)
max_xx = xx; max_xx = xx;
}
xx = x; xx = x;
yy += fheight + ys; yy += fheight + ys;
} }
@ -1439,26 +1384,25 @@ 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) {
// begin format code?
fcode = 1; fcode = 1;
fptr = 0; 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;
@ -1467,27 +1411,25 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
str++; str++;
continue; continue;
} }
if(*str != '<' && *str != '>' && fcode == 1) if (*str != '<' && *str != '>' && fcode == 1) {
{
// Add to the format stack (up to 10 bytes.) // Add to the format stack (up to 10 bytes.)
if(fptr > 10) // stop adding bytes if (fptr > 10) {
{ // stop adding bytes
str++; // go to next byte str++; // go to next byte
continue; continue;
} }
fstack[fptr++] = *str; fstack[fptr++] = *str;
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
} }
if(fcode == 0) if (fcode == 0) {
{
// Not a format code, raw text. So we draw it. // Not a format code, raw text. So we draw it.
// TODO - different font sizes. // TODO - different font sizes.
write_char(*str, xx, yy + (max_height - fheight), flags, font); write_char(*str, xx, yy + (max_height - fheight), flags, font);
xx += fwidth + xs; xx += fwidth + xs;
if(*str == '\n') if (*str == '\n') {
{ if (xx > max_xx) {
if(xx > max_xx)
max_xx = xx; max_xx = xx;
}
xx = x; xx = x;
yy += fheight + ys; yy += fheight + ys;
} }
@ -1496,10 +1438,8 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
} }
} }
//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)
@ -1525,19 +1465,18 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t
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; a = (a * (size / 2)) / 100;
b = (b * (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; k = a * pitch / 90;
l = b * pitch / 90; l = b * pitch / 90;
@ -1562,7 +1501,6 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t
//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 //roll
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line //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 write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line
@ -1576,7 +1514,6 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t
//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 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, 5); //drawCircle(x-1, y-1, 5);
//write_circle_outlined(x-1, y-1, 5,0,0,0,1); //write_circle_outlined(x-1, y-1, 5,0,0,0,1);
//drawCircle(x-1, y-1, size/2+4); //drawCircle(x-1, y-1, size/2+4);
@ -1608,15 +1545,15 @@ void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
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] =
{ 0 };
sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec); sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
//printTextFB(x,y,temp); //printTextFB(x,y,temp);
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
@ -1655,7 +1592,8 @@ 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;
@ -1664,17 +1602,14 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
//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) {
else if(halign == +1)
{
x = x - GRAPHICS_HDEADBAND; x = x - GRAPHICS_HDEADBAND;
majtick_start = GRAPHICS_WIDTH_REAL - x - 1; majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
@ -1693,8 +1628,7 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
int range_2 = range / 2; //, height_2 = height / 2; int range_2 = range / 2; //, height_2 = height / 2;
int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
// Iterate through each step. // Iterate through each step.
for(r = -range_2; r <= +range_2; r++) for (r = -range_2; r <= +range_2; r++) {
{
style = 0; style = 0;
rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
rv = -rr + range_2; // for number display rv = -rr + range_2; // for number display
@ -1708,15 +1642,13 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
style = 0; style = 0;
if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
continue; continue;
if(style) if (style) {
{
// Calculate y position. // Calculate y position.
ys = ((long int) (r * height) / (long int) range) + y; ys = ((long int) (r * height) / (long int) range) + y;
//sprintf(temp, "ys=%d", ys); //sprintf(temp, "ys=%d", ys);
//con_puts(temp, 0); //con_puts(temp, 0);
// Depending on style, draw a minor or a major tick. // Depending on style, draw a minor or a major tick.
if(style == 1) if (style == 1) {
{
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1); write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
memset(temp, ' ', 10); memset(temp, ' ', 10);
//my_itoa(rv, temp); //my_itoa(rv, temp);
@ -1728,8 +1660,7 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1); write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
else else
write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1); write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
} } else if (style == 2)
else if(style == 2)
write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1); write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
} }
} }
@ -1745,17 +1676,13 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
else else
xx = majtick_end - text_x_spacing; xx = majtick_end - text_x_spacing;
// Draw an arrow from the number to the point. // Draw an arrow from the number to the point.
for(i = 0; i < arrow_len; i++) 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 {
else
{
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);
@ -1765,14 +1692,11 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
// 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);
// 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) if (halign == -1) {
{
write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1); write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
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 - 2, 1, 1);
write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
} } else {
else
{
write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1); write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
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 - 2, 1, 1);
write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
@ -1785,13 +1709,12 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
// Then, add a slow cut off on the edges, so the text doesn't sharply // Then, add a slow cut off on the edges, so the text doesn't sharply
// disappear. We simply clear the areas above and below the ticker, and we // disappear. We simply clear the areas above and below the ticker, and we
// use little markers on the edges. // use little markers on the edges.
if(halign == -1) 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,
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, 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);
else } 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);
} }
@ -1826,8 +1749,7 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
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
@ -1835,33 +1757,35 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
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. // Calculate x position.
xs = ((long int) (r * width) / (long int) range) + x; xs = ((long int) (r * width) / (long int) range) + x;
// Draw it. // Draw it.
if(style == 1) if (style == 1) {
{
write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
// Draw heading above this tick. // Draw heading above this tick.
// If it's not one of north, south, east, west, draw the heading. // If it's not one of north, south, east, west, draw the heading.
// Otherwise, draw one of the identifiers. // Otherwise, draw one of the identifiers.
if(rr % 90 != 0) if (rr % 90 != 0) {
{
// We abbreviate heading to two digits. This has the side effect of being easy to compute. // We abbreviate heading to two digits. This has the side effect of being easy to compute.
headingstr[0] = '0' + (rr / 100); headingstr[0] = '0' + (rr / 100);
headingstr[1] = '0' + ((rr / 10) % 10); headingstr[1] = '0' + ((rr / 10) % 10);
headingstr[2] = 0; headingstr[2] = 0;
headingstr[3] = 0; // nul to terminate headingstr[3] = 0; // nul to terminate
} } else {
else switch (rr) {
{ case 0:
switch(rr) headingstr[0] = 'N';
{ break;
case 0: headingstr[0] = 'N'; break; case 90:
case 90: headingstr[0] = 'E'; break; headingstr[0] = 'E';
case 180: headingstr[0] = 'S'; break; break;
case 270: headingstr[0] = 'W'; break; case 180:
headingstr[0] = 'S';
break;
case 270:
headingstr[0] = 'W';
break;
} }
headingstr[1] = 0; headingstr[1] = 0;
headingstr[2] = 0; headingstr[2] = 0;
@ -1869,8 +1793,7 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
} }
// +1 fudge...! // +1 fudge...!
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1); write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
} } else if (style == 2)
else if(style == 2)
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1); write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
} }
} }
@ -1908,36 +1831,29 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
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 {
else
{
vertical = 1; 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; y1 = 0;
x1 = ((y1 - y0) + k * x0) / k; x1 = ((y1 - y0) + k * x0) / k;
} }
if(y>size) if (y > size) {
{
y1 = size; y1 = size;
x1 = ((y1 - y0) + k * x0) / k; x1 = ((y1 - y0) + k * x0) / k;
} }
@ -1946,13 +1862,11 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
y = k * (x - x0) + y0; y = k * (x - x0) + y0;
x2 = x; x2 = x;
y2 = y; y2 = y;
if(y<0) if (y < 0) {
{
y2 = 0; y2 = 0;
x2 = ((y2 - y0) + k * x0) / k; x2 = ((y2 - y0) + k * x0) / k;
} }
if(y>size) if (y > size) {
{
y2 = size; y2 = size;
x2 = ((y2 - y0) + k * x0) / k; x2 = ((y2 - y0) + k * x0) / k;
} }
@ -1960,97 +1874,80 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
// horizon line // horizon line
write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1); write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
//fill //fill
if(angle<=0 && angle>-90) if (angle <= 0 && angle > -90) {
{
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
for(int i=y2;i<size;i++) for (int i = y2; i < size; i++) {
{
x2 = ((i - y0) + k * x0) / k; x2 = ((i - y0) + k * x0) / k;
if(x2>size) if (x2 > size) {
x2 = size; x2 = size;
if(x2<0) }
if (x2 < 0) {
x2 = 0; x2 = 0;
}
write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1); write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
} }
} } else if (angle < -90) {
else if(angle<-90)
{
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("2", 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<y2;i++) for (int i = 0; i < y2; i++) {
{
x2 = ((i - y0) + k * x0) / k; x2 = ((i - y0) + k * x0) / k;
if(x2>size) if (x2 > size) {
x2 = size; x2 = size;
if(x2<0) }
if (x2 < 0) {
x2 = 0; x2 = 0;
}
write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1); write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
} }
} } else if (angle > 0 && angle < 90) {
else if(angle>0 && angle<90)
{
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //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++) for (int i = y1; i < size; i++) {
{
x2 = ((i - y0) + k * x0) / k; x2 = ((i - y0) + k * x0) / k;
if(x2>size) if (x2 > size) {
x2 = size; x2 = size;
if(x2<0) }
if (x2 < 0) {
x2 = 0; x2 = 0;
}
write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1); write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
} }
} } else if (angle > 90) {
else if(angle>90)
{
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("4", 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<y1;i++) for (int i = 0; i < y1; i++) {
{
x2 = ((i - y0) + k * x0) / k; x2 = ((i - y0) + k * x0) / k;
if(x2>size) if (x2 > size) {
x2 = size; x2 = size;
if(x2<0) }
if (x2 < 0) {
x2 = 0; x2 = 0;
}
write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1); write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
} }
} }
} } else if (vertical) {
else if(vertical)
{
// horizon line // horizon line
write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1); write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
if(angle==90) if (angle == 90) {
{
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); //write_string("5", 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 = 0; i < size; i++) {
{
write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1); write_hline_lm(0 + l_x, x0 + 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("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++) for (int i = 0; i < size; i++) {
{
write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1); write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
} }
} }
} } else if (horizontal) {
else if(horizontal)
{
// horizon line // horizon line
write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1); write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
if(angle<0) 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); //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++) for (int i = 0; i < y0; i++) {
{
write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
} }
} } else {
else
{
//write_string("8", 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=y0;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(0 + l_x, size + l_x, i + l_y, 1, 1);
} }
} }
@ -2064,12 +1961,13 @@ void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y,
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 */ /* logo */
int image = 0; int image = 0;
struct splashEntry splash_info; struct splashEntry splash_info;
@ -2117,13 +2015,15 @@ void calcHomeArrow(int16_t m_yaw)
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
/** /**
@ -2136,9 +2036,7 @@ void calcHomeArrow(int16_t m_yaw)
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) *
sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
c = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); c = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
d = 6371 * 1000 * c; d = 6371 * 1000 * c;
@ -2149,7 +2047,8 @@ void calcHomeArrow(int16_t m_yaw)
elevation = 0; elevation = 0;
//! TODO: sanity check //! TODO: sanity check
char temp[50]={0}; char temp[50] =
{ 0 };
sprintf(temp, "hea:%d", (int) brng); sprintf(temp, "hea:%d", (int) brng);
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), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
sprintf(temp, "ele:%d", (int) elevation); sprintf(temp, "ele:%d", (int) elevation);
@ -2168,26 +2067,25 @@ int lama_loc[2][30];
void lamas(void) void lamas(void)
{ {
char temp[10]={0}; char temp[10] =
{ 0 };
lama++; lama++;
if(lama%10==0) 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; OsdSettingsData OsdSettings;
OsdSettingsGet(&OsdSettings); OsdSettingsGet(&OsdSettings);
AttitudeActualData attitude; AttitudeActualData attitude;
@ -2205,16 +2103,16 @@ void updateGraphics() {
switch (OsdSettings.Screen) { switch (OsdSettings.Screen) {
case 0: // Dave simple case 0: // Dave simple
{ {
if(home.Set == HOMELOCATION_SET_FALSE) if (home.Set == HOMELOCATION_SET_FALSE) {
{ char temps[20] =
char temps[20]={0}; { 0 };
sprintf(temps, "HOME NOT SET"); sprintf(temps, "HOME NOT SET");
//printTextFB(x,y,temp); //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); write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
} }
char temp[50] =
char temp[50]={0}; { 0 };
memset(temp, ' ', 40); memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f); 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); write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
@ -2265,14 +2163,15 @@ void updateGraphics() {
calcHomeArrow((int16_t)(gpsData.Heading)); calcHomeArrow((int16_t)(gpsData.Heading));
/* Draw Attitude Indicator */ /* Draw Attitude Indicator */
if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
{ drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96); 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); //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
//printText16( 60, 12,"Hello OP-OSD"); //printText16( 60, 12,"Hello OP-OSD");
char temp[50]={0}; char temp[50] =
{ 0 };
memset(temp, ' ', 40); memset(temp, ' ', 40);
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f); 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); write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
@ -2283,10 +2182,8 @@ void updateGraphics() {
sprintf(temp, "Sat:%d", (int) gpsData.Satellites); 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); write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
/* Print RTC time */ /* Print RTC time */
if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
{
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
} }
@ -2315,7 +2212,6 @@ void updateGraphics() {
/* Print ADC voltage RSSI */ /* Print ADC voltage RSSI */
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); //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); //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
/* Draw Battery Gauge */ /* Draw Battery Gauge */
/*m_batt++; /*m_batt++;
uint8_t dir=3; uint8_t dir=3;
@ -2339,24 +2235,19 @@ void updateGraphics() {
//drawAltitude(200,50,m_alt,dir); //drawAltitude(200,50,m_alt,dir);
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.) // Draw airspeed (left side.)
if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
{
hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), 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); APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
} }
// Draw altimeter (right side.) // Draw altimeter (right side.)
if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
{
hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), 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); APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
} }
// Draw compass. // Draw compass.
if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
{
if (attitude.Yaw < 0) { if (attitude.Yaw < 0) {
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), 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); APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
@ -2372,17 +2263,13 @@ void updateGraphics() {
int size = 64; int size = 64;
int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); 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); 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)), 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,
APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
if(OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) 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);
hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), } else {
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); 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);
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);
} }
} }
@ -2416,12 +2303,12 @@ void updateGraphics() {
} }
} }
void updateOnceEveryFrame() { void updateOnceEveryFrame()
{
clearGraphics(); clearGraphics();
updateGraphics(); updateGraphics();
} }
// **************** // ****************
/** /**
* Initialise the gps module * Initialise the gps module
@ -2481,28 +2368,22 @@ static void osdgenTask(void *parameters)
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(); clearGraphics();
introGraphics(); 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(); clearGraphics();
introGraphics(); introGraphics();
introText(); 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);
@ -2510,7 +2391,6 @@ static void osdgenTask(void *parameters)
} }
} }
// **************** // ****************
/** /**

171
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);
@ -170,11 +169,9 @@ static void vtolPathFollowerTask(void *parameters)
portTickType lastUpdateTime; portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
AccessoryDesiredConnectCallback(accessoryUpdated); AccessoryDesiredConnectCallback(accessoryUpdated);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop // Main task loop
lastUpdateTime = xTaskGetTickCount(); lastUpdateTime = xTaskGetTickCount();
@ -186,18 +183,12 @@ static void vtolPathFollowerTask(void *parameters)
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings); SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
(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); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000); vTaskDelay(1000);
continue; continue;
@ -211,6 +202,8 @@ static void vtolPathFollowerTask(void *parameters)
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus); PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode // Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
@ -288,6 +281,12 @@ static void vtolPathFollowerTask(void *parameters)
*/ */
static void updatePOIBearing() static void updatePOIBearing()
{ {
const float DEADBAND_HIGH = 0.10;
const float DEADBAND_LOW = -0.10;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
CameraDesiredData cameraDesired; CameraDesiredData cameraDesired;
@ -296,9 +295,6 @@ static void updatePOIBearing()
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
PoiLocationData poi; PoiLocationData poi;
PoiLocationGet(&poi); PoiLocationGet(&poi);
//use poi here
//HomeLocationData poi;
//HomeLocationGet (&poi);
float dLoc[3]; float dLoc[3];
float yaw = 0; float yaw = 0;
@ -308,28 +304,63 @@ static void updatePOIBearing()
dLoc[1] = positionActual.East - poi.East; dLoc[1] = positionActual.East - poi.East;
dLoc[2] = positionActual.Down - poi.Down; dLoc[2] = positionActual.Down - poi.Down;
if(dLoc[1]<0) if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180; yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180;
else } else {
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180; yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
}
// distance // distance
float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2)); float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
ManualControlCommandData manualControlData;
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 //not above
if(distance!=0) { if (distance >= 3.0f) {
//You can feed this into camerastabilization //You can feed this into camerastabilization
elevation = RAD2DEG(atan2f(dLoc[2],distance)); elevation = RAD2DEG(atan2f(dLoc[2],distance));
}
stabDesired.Yaw=yaw; stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
cameraDesired.Yaw=yaw;
cameraDesired.PitchOrServo2=elevation; //cameraDesired.Yaw=yaw;
//cameraDesired.PitchOrServo2=elevation;
CameraDesiredSet(&cameraDesired); CameraDesiredSet(&cameraDesired);
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }
}
/** /**
* Compute desired velocity from the current position and path * Compute desired velocity from the current position and path
@ -342,10 +373,13 @@ static void updatePathVelocity()
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand; float downCommand;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; float cur[3] =
{ positionActual.North, positionActual.East, positionActual.Down };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
@ -360,16 +394,15 @@ static void updatePathVelocity()
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
bound(progress.fractional_progress,0,1);
if (progress.fractional_progress > 1) if (progress.fractional_progress > 1)
groundspeed = 0; groundspeed = 0;
break; break;
case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR:
default: default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * groundspeed = pathDesired.StartingVelocity
bound(progress.fractional_progress,0,1); + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) if (progress.fractional_progress > 1)
groundspeed = 0; groundspeed = 0;
break; break;
@ -380,8 +413,8 @@ static void updatePathVelocity()
velocityDesired.East = progress.path_direction[1] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed, float correction_velocity[2] =
progress.correction_direction[1] * error_speed}; { 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 total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
float scale = 1; float scale = 1;
@ -391,17 +424,14 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
bound(progress.fractional_progress,0,1);
float downError = altitudeSetpoint - positionActual.Down; float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus // update pathstatus
pathStatus.error = progress.error; pathStatus.error = progress.error;
@ -420,6 +450,9 @@ void updateEndpointVelocity()
{ {
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
PositionActualData positionActual; PositionActualData positionActual;
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
@ -450,12 +483,12 @@ void updateEndpointVelocity()
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
float alt = homeLocation.Altitude; float alt = homeLocation.Altitude;
float T[3] = { alt+6.378137E6f, float T[3] =
cosf(lat)*(alt+6.378137E6f), { alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f };
-1.0f}; float NED[3] =
float NED[3] = {T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), { T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1]
T[1] * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2]
T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))}; * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
northPos = NED[0]; northPos = NED[0];
eastPos = NED[1]; eastPos = NED[1];
@ -472,15 +505,13 @@ void updateEndpointVelocity()
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
eastPosIntegral);
// Limit the maximum velocity // Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
@ -496,9 +527,7 @@ void updateEndpointVelocity()
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand, velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired); VelocityDesiredSet(&velocityDesired);
} }
@ -600,20 +629,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command // Compute desired east command
eastError = velocityDesired.East - eastVel; eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command // Compute desired down command
downError = velocityDesired.Down - downVel; downError = velocityDesired.Down - downVel;
@ -622,19 +649,16 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
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) + stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
@ -711,7 +735,6 @@ static float bound(float val, float min, float max)
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)
@ -719,17 +742,17 @@ static void accessoryUpdated(UAVObjEvent* ev)
if (ev->obj != AccessoryDesiredHandle()) if (ev->obj != AccessoryDesiredHandle())
return; return;
PositionActualData positionActual;
PositionActualGet(&positionActual);
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
PoiLearnSettingsData poiLearn; PoiLearnSettingsData poiLearn;
PoiLearnSettingsGet(&poiLearn); PoiLearnSettingsGet(&poiLearn);
PoiLocationData poi;
PoiLocationGet(&poi);
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
if(accessory.AccessoryVal<-0.5f) if (accessory.AccessoryVal < -0.5f) {
{ PositionActualData positionActual;
PositionActualGet(&positionActual);
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;

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

@ -49,11 +49,12 @@ 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();
@ -103,8 +100,9 @@ void AeroSimRCSimulator::transmitUpdate()
} }
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[2] = throttle;
channels[3] = yaw; channels[3] = yaw;
@ -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
@ -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